Bullet Collision Detection & Physics Library: btMultiBodyDynamicsWorld.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyDynamicsWorld.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "btMultiBodyDynamicsWorld.h"
17 #include "btMultiBodyConstraintSolver.h"
18 #include "btMultiBody.h"
19 #include "btMultiBodyLinkCollider.h"
20 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
21 #include "LinearMath/btQuickprof.h"
22 #include "btMultiBodyConstraint.h"
23 #include "LinearMath/btIDebugDraw.h"
24
25
26
27 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
28 {
29 m_multiBodies.push_back(body);
30
31 }
32
33 void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
34 {
35 m_multiBodies.remove(body);
36 }
37
38 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
39 {
40 BT_PROFILE("calculateSimulationIslands");
41
42 getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
43
44 {
45 //merge islands based on speculative contact manifolds too
46 for (int i=0;i<this->m_predictiveManifolds.size();i++)
47 {
48 btPersistentManifold* manifold = m_predictiveManifolds[i];
49
50 const btCollisionObject* colObj0 = manifold->getBody0();
51 const btCollisionObject* colObj1 = manifold->getBody1();
52
53 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
54 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
55 {
56 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
57 }
58 }
59 }
60
61 {
62 int i;
63 int numConstraints = int(m_constraints.size());
64 for (i=0;i< numConstraints ; i++ )
65 {
66 btTypedConstraint* constraint = m_constraints[i];
67 if (constraint->isEnabled())
68 {
69 const btRigidBody* colObj0 = &constraint->getRigidBodyA();
70 const btRigidBody* colObj1 = &constraint->getRigidBodyB();
71
72 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
73 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
74 {
75 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
76 }
77 }
78 }
79 }
80
81 //merge islands linked by Featherstone link colliders
82 for (int i=0;i<m_multiBodies.size();i++)
83 {
84 btMultiBody* body = m_multiBodies[i];
85 {
86 btMultiBodyLinkCollider* prev = body->getBaseCollider();
87
88 for (int b=0;b<body->getNumLinks();b++)
89 {
90 btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
91
92 if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
93 ((prev) && (!(prev)->isStaticOrKinematicObject())))
94 {
95 int tagPrev = prev->getIslandTag();
96 int tagCur = cur->getIslandTag();
97 getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
98 }
99 if (cur && !cur->isStaticOrKinematicObject())
100 prev = cur;
101
102 }
103 }
104 }
105
106 //merge islands linked by multibody constraints
107 {
108 for (int i=0;i<this->m_multiBodyConstraints.size();i++)
109 {
110 btMultiBodyConstraint* c = m_multiBodyConstraints[i];
111 int tagA = c->getIslandIdA();
112 int tagB = c->getIslandIdB();
113 if (tagA>=0 && tagB>=0)
114 getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
115 }
116 }
117
118 //Store the island id in each body
119 getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
120
121 }
122
123
124 void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
125 {
126 BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
127
128
129
130 for ( int i=0;i<m_multiBodies.size();i++)
131 {
132 btMultiBody* body = m_multiBodies[i];
133 if (body)
134 {
135 body->checkMotionAndSleepIfRequired(timeStep);
136 if (!body->isAwake())
137 {
138 btMultiBodyLinkCollider* col = body->getBaseCollider();
139 if (col && col->getActivationState() == ACTIVE_TAG)
140 {
141 col->setActivationState( WANTS_DEACTIVATION);
142 col->setDeactivationTime(0.f);
143 }
144 for (int b=0;b<body->getNumLinks();b++)
145 {
146 btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
147 if (col && col->getActivationState() == ACTIVE_TAG)
148 {
149 col->setActivationState( WANTS_DEACTIVATION);
150 col->setDeactivationTime(0.f);
151 }
152 }
153 } else
154 {
155 btMultiBodyLinkCollider* col = body->getBaseCollider();
156 if (col && col->getActivationState() != DISABLE_DEACTIVATION)
157 col->setActivationState( ACTIVE_TAG );
158
159 for (int b=0;b<body->getNumLinks();b++)
160 {
161 btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
162 if (col && col->getActivationState() != DISABLE_DEACTIVATION)
163 col->setActivationState( ACTIVE_TAG );
164 }
165 }
166
167 }
168 }
169
170 btDiscreteDynamicsWorld::updateActivationState(timeStep);
171 }
172
173
174 SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
175 {
176 int islandId;
177
178 const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
179 const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
180 islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
181 return islandId;
182
183 }
184
185
186 class btSortConstraintOnIslandPredicate2
187 {
188 public:
189
190 bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
191 {
192 int rIslandId0,lIslandId0;
193 rIslandId0 = btGetConstraintIslandId2(rhs);
194 lIslandId0 = btGetConstraintIslandId2(lhs);
195 return lIslandId0 < rIslandId0;
196 }
197 };
198
199
200
201 SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
202 {
203 int islandId;
204
205 int islandTagA = lhs->getIslandIdA();
206 int islandTagB = lhs->getIslandIdB();
207 islandId= islandTagA>=0?islandTagA:islandTagB;
208 return islandId;
209
210 }
211
212
213 class btSortMultiBodyConstraintOnIslandPredicate
214 {
215 public:
216
217 bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
218 {
219 int rIslandId0,lIslandId0;
220 rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
221 lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
222 return lIslandId0 < rIslandId0;
223 }
224 };
225
226 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
227 {
228 btContactSolverInfo* m_solverInfo;
229 btMultiBodyConstraintSolver* m_solver;
230 btMultiBodyConstraint** m_multiBodySortedConstraints;
231 int m_numMultiBodyConstraints;
232
233 btTypedConstraint** m_sortedConstraints;
234 int m_numConstraints;
235 btIDebugDraw* m_debugDrawer;
236 btDispatcher* m_dispatcher;
237
238 btAlignedObjectArray<btCollisionObject*> m_bodies;
239 btAlignedObjectArray<btPersistentManifold*> m_manifolds;
240 btAlignedObjectArray<btTypedConstraint*> m_constraints;
241 btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
242
243
244 MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver,
245 btDispatcher* dispatcher)
246 :m_solverInfo(NULL),
247 m_solver(solver),
248 m_multiBodySortedConstraints(NULL),
249 m_numConstraints(0),
250 m_debugDrawer(NULL),
251 m_dispatcher(dispatcher)
252 {
253
254 }
255
256 MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
257 {
258 btAssert(0);
259 (void)other;
260 return *this;
261 }
262
263 SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
264 {
265 btAssert(solverInfo);
266 m_solverInfo = solverInfo;
267
268 m_multiBodySortedConstraints = sortedMultiBodyConstraints;
269 m_numMultiBodyConstraints = numMultiBodyConstraints;
270 m_sortedConstraints = sortedConstraints;
271 m_numConstraints = numConstraints;
272
273 m_debugDrawer = debugDrawer;
274 m_bodies.resize (0);
275 m_manifolds.resize (0);
276 m_constraints.resize (0);
277 m_multiBodyConstraints.resize(0);
278 }
279
280
281 virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
282 {
283 if (islandId<0)
284 {
286 m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
287 } else
288 {
289 //also add all non-contact constraints/joints for this island
290 btTypedConstraint** startConstraint = 0;
291 btMultiBodyConstraint** startMultiBodyConstraint = 0;
292
293 int numCurConstraints = 0;
294 int numCurMultiBodyConstraints = 0;
295
296 int i;
297
298 //find the first constraint for this island
299
300 for (i=0;i<m_numConstraints;i++)
301 {
302 if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
303 {
304 startConstraint = &m_sortedConstraints[i];
305 break;
306 }
307 }
308 //count the number of constraints in this island
309 for (;i<m_numConstraints;i++)
310 {
311 if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
312 {
313 numCurConstraints++;
314 }
315 }
316
317 for (i=0;i<m_numMultiBodyConstraints;i++)
318 {
319 if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
320 {
321
322 startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
323 break;
324 }
325 }
326 //count the number of multi body constraints in this island
327 for (;i<m_numMultiBodyConstraints;i++)
328 {
329 if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
330 {
331 numCurMultiBodyConstraints++;
332 }
333 }
334
335 if (m_solverInfo->m_minimumSolverBatchSize<=1)
336 {
337 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
338 } else
339 {
340
341 for (i=0;i<numBodies;i++)
342 m_bodies.push_back(bodies[i]);
343 for (i=0;i<numManifolds;i++)
344 m_manifolds.push_back(manifolds[i]);
345 for (i=0;i<numCurConstraints;i++)
346 m_constraints.push_back(startConstraint[i]);
347
348 for (i=0;i<numCurMultiBodyConstraints;i++)
349 m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
350
351 if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
352 {
353 processConstraints();
354 } else
355 {
356 //printf("deferred\n");
357 }
358 }
359 }
360 }
361 void processConstraints()
362 {
363
364 btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
365 btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
366 btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
367 btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
368
369 //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
370
371 m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
372 m_bodies.resize(0);
373 m_manifolds.resize(0);
374 m_constraints.resize(0);
375 m_multiBodyConstraints.resize(0);
376 }
377
378 };
379
380
381
382 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
383 :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
384 m_multiBodyConstraintSolver(constraintSolver)
385 {
386 //split impulse is not yet supported for Featherstone hierarchies
387 getSolverInfo().m_splitImpulse = false;
388 getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
389 m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
390 }
391
392 btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
393 {
394 delete m_solverMultiBodyIslandCallback;
395 }
396
397 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
398 {
399
400 btAlignedObjectArray<btScalar> scratch_r;
401 btAlignedObjectArray<btVector3> scratch_v;
402 btAlignedObjectArray<btMatrix3x3> scratch_m;
403
404
405 BT_PROFILE("solveConstraints");
406
407 m_sortedConstraints.resize( m_constraints.size());
408 int i;
409 for (i=0;i<getNumConstraints();i++)
410 {
411 m_sortedConstraints[i] = m_constraints[i];
412 }
413 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
414 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
415
416 m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
417 for (i=0;i<m_multiBodyConstraints.size();i++)
418 {
419 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
420 }
421 m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
422
423 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
424
425
426 m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
427 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
428
430 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
431
432
433 {
434 BT_PROFILE("btMultiBody addForce and stepVelocities");
435 for (int i=0;i<this->m_multiBodies.size();i++)
436 {
437 btMultiBody* bod = m_multiBodies[i];
438
439 bool isSleeping = false;
440
441 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
442 {
443 isSleeping = true;
444 }
445 for (int b=0;b<bod->getNumLinks();b++)
446 {
447 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
448 isSleeping = true;
449 }
450
451 if (!isSleeping)
452 {
453 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
454 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
455 scratch_v.resize(bod->getNumLinks()+1);
456 scratch_m.resize(bod->getNumLinks()+1);
457
458 bod->addBaseForce(m_gravity * bod->getBaseMass());
459
460 for (int j = 0; j < bod->getNumLinks(); ++j)
461 {
462 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
463 }
464
465 bool doNotUpdatePos = false;
466
467 if(bod->isMultiDof())
468 {
469 if(!bod->isUsingRK4Integration())
470 {
471 bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
472 }
473 else
474 {
475 //
476 int numDofs = bod->getNumDofs() + 6;
477 int numPosVars = bod->getNumPosVars() + 7;
478 btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
479 //convenience
480 btScalar *pMem = &scratch_r2[0];
481 btScalar *scratch_q0 = pMem; pMem += numPosVars;
482 btScalar *scratch_qx = pMem; pMem += numPosVars;
483 btScalar *scratch_qd0 = pMem; pMem += numDofs;
484 btScalar *scratch_qd1 = pMem; pMem += numDofs;
485 btScalar *scratch_qd2 = pMem; pMem += numDofs;
486 btScalar *scratch_qd3 = pMem; pMem += numDofs;
487 btScalar *scratch_qdd0 = pMem; pMem += numDofs;
488 btScalar *scratch_qdd1 = pMem; pMem += numDofs;
489 btScalar *scratch_qdd2 = pMem; pMem += numDofs;
490 btScalar *scratch_qdd3 = pMem; pMem += numDofs;
491 btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
492
494 //copy q0 to scratch_q0 and qd0 to scratch_qd0
495 scratch_q0[0] = bod->getWorldToBaseRot().x();
496 scratch_q0[1] = bod->getWorldToBaseRot().y();
497 scratch_q0[2] = bod->getWorldToBaseRot().z();
498 scratch_q0[3] = bod->getWorldToBaseRot().w();
499 scratch_q0[4] = bod->getBasePos().x();
500 scratch_q0[5] = bod->getBasePos().y();
501 scratch_q0[6] = bod->getBasePos().z();
502 //
503 for(int link = 0; link < bod->getNumLinks(); ++link)
504 {
505 for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
506 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
507 }
508 //
509 for(int dof = 0; dof < numDofs; ++dof)
510 scratch_qd0[dof] = bod->getVelocityVector()[dof];
512 struct
513 {
514 btMultiBody *bod;
515 btScalar *scratch_qx, *scratch_q0;
516
517 void operator()()
518 {
519 for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
520 scratch_qx[dof] = scratch_q0[dof];
521 }
522 } pResetQx = {bod, scratch_qx, scratch_q0};
523 //
524 struct
525 {
526 void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
527 {
528 for(int i = 0; i < size; ++i)
529 pVal[i] = pCurVal[i] + dt * pDer[i];
530 }
531
532 } pEulerIntegrate;
533 //
534 struct
535 {
536 void operator()(btMultiBody *pBody, const btScalar *pData)
537 {
538 btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
539
540 for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
541 pVel[i] = pData[i];
542
543 }
544 } pCopyToVelocityVector;
545 //
546 struct
547 {
548 void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
549 {
550 for(int i = 0; i < size; ++i)
551 pDst[i] = pSrc[start + i];
552 }
553 } pCopy;
554 //
555
556 btScalar h = solverInfo.m_timeStep;
557 #define output &scratch_r[bod->getNumDofs()]
558 //calc qdd0 from: q0 & qd0
559 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
560 pCopy(output, scratch_qdd0, 0, numDofs);
561 //calc q1 = q0 + h/2 * qd0
562 pResetQx();
563 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
564 //calc qd1 = qd0 + h/2 * qdd0
565 pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
566 //
567 //calc qdd1 from: q1 & qd1
568 pCopyToVelocityVector(bod, scratch_qd1);
569 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
570 pCopy(output, scratch_qdd1, 0, numDofs);
571 //calc q2 = q0 + h/2 * qd1
572 pResetQx();
573 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
574 //calc qd2 = qd0 + h/2 * qdd1
575 pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
576 //
577 //calc qdd2 from: q2 & qd2
578 pCopyToVelocityVector(bod, scratch_qd2);
579 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
580 pCopy(output, scratch_qdd2, 0, numDofs);
581 //calc q3 = q0 + h * qd2
582 pResetQx();
583 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
584 //calc qd3 = qd0 + h * qdd2
585 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
586 //
587 //calc qdd3 from: q3 & qd3
588 pCopyToVelocityVector(bod, scratch_qd3);
589 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
590 pCopy(output, scratch_qdd3, 0, numDofs);
591
592 //
593 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
594 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
595 btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
596 btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
597 for(int i = 0; i < numDofs; ++i)
598 {
599 delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
600 delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
601 //delta_q[i] = h*scratch_qd0[i];
602 //delta_qd[i] = h*scratch_qdd0[i];
603 }
604 //
605 pCopyToVelocityVector(bod, scratch_qd0);
606 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
607 //
608 if(!doNotUpdatePos)
609 {
610 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
611 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
612
613 for(int i = 0; i < numDofs; ++i)
614 pRealBuf[i] = delta_q[i];
615
616 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
617 bod->setPosUpdated(true);
618 }
619
620 //ugly hack which resets the cached data to t0 (needed for constraint solver)
621 {
622 for(int link = 0; link < bod->getNumLinks(); ++link)
623 bod->getLink(link).updateCacheMultiDof();
624 bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
625 }
626
627 }
628 }
629 else//if(bod->isMultiDof())
630 {
631 bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
632 }
633 bod->clearForcesAndTorques();
634 }//if (!isSleeping)
635 }
636 }
637
638 m_solverMultiBodyIslandCallback->processConstraints();
639
640 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
641
642 }
643
644 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
645 {
646 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
647
648 {
649 BT_PROFILE("btMultiBody stepPositions");
650 //integrate and update the Featherstone hierarchies
651 btAlignedObjectArray<btQuaternion> world_to_local;
652 btAlignedObjectArray<btVector3> local_origin;
653
654 for (int b=0;b<m_multiBodies.size();b++)
655 {
656 btMultiBody* bod = m_multiBodies[b];
657 bool isSleeping = false;
658 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
659 {
660 isSleeping = true;
661 }
662 for (int b=0;b<bod->getNumLinks();b++)
663 {
664 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
665 isSleeping = true;
666 }
667
668
669 if (!isSleeping)
670 {
671 int nLinks = bod->getNumLinks();
672
674 world_to_local.resize(nLinks+1);
675 local_origin.resize(nLinks+1);
676
677 if(bod->isMultiDof())
678 {
679 if(!bod->isPosUpdated())
680 bod->stepPositionsMultiDof(timeStep);
681 else
682 {
683 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
684 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
685
686 bod->stepPositionsMultiDof(1, 0, pRealBuf);
687 bod->setPosUpdated(false);
688 }
689 }
690 else
691 bod->stepPositions(timeStep);
692
693 world_to_local[0] = bod->getWorldToBaseRot();
694 local_origin[0] = bod->getBasePos();
695
696 if (bod->getBaseCollider())
697 {
698 btVector3 posr = local_origin[0];
699 // float pos[4]={posr.x(),posr.y(),posr.z(),1};
700 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
701 btTransform tr;
702 tr.setIdentity();
703 tr.setOrigin(posr);
704 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
705
706 bod->getBaseCollider()->setWorldTransform(tr);
707
708 }
709
710 for (int k=0;k<bod->getNumLinks();k++)
711 {
712 const int parent = bod->getParent(k);
713 world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
714 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
715 }
716
717
718 for (int m=0;m<bod->getNumLinks();m++)
719 {
720 btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
721 if (col)
722 {
723 int link = col->m_link;
724 btAssert(link == m);
725
726 int index = link+1;
727
728 btVector3 posr = local_origin[index];
729 // float pos[4]={posr.x(),posr.y(),posr.z(),1};
730 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
731 btTransform tr;
732 tr.setIdentity();
733 tr.setOrigin(posr);
734 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
735
736 col->setWorldTransform(tr);
737 }
738 }
739 } else
740 {
741 bod->clearVelocities();
742 }
743 }
744 }
745 }
746
747
748
749 void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
750 {
751 m_multiBodyConstraints.push_back(constraint);
752 }
753
754 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
755 {
756 m_multiBodyConstraints.remove(constraint);
757 }
758
759 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
760 {
761 constraint->debugDraw(getDebugDrawer());
762 }
763
764
765 void btMultiBodyDynamicsWorld::debugDrawWorld()
766 {
767 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
768
769 bool drawConstraints = false;
770 if (getDebugDrawer())
771 {
772 int mode = getDebugDrawer()->getDebugMode();
773 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
774 {
775 drawConstraints = true;
776 }
777
778 if (drawConstraints)
779 {
780 BT_PROFILE("btMultiBody debugDrawWorld");
781
782 btAlignedObjectArray<btQuaternion> world_to_local;
783 btAlignedObjectArray<btVector3> local_origin;
784
785 for (int c=0;c<m_multiBodyConstraints.size();c++)
786 {
787 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
788 debugDrawMultiBodyConstraint(constraint);
789 }
790
791 for (int b = 0; b<m_multiBodies.size(); b++)
792 {
793 btMultiBody* bod = m_multiBodies[b];
794 int nLinks = bod->getNumLinks();
795
797 world_to_local.resize(nLinks + 1);
798 local_origin.resize(nLinks + 1);
799
800
801 world_to_local[0] = bod->getWorldToBaseRot();
802 local_origin[0] = bod->getBasePos();
803
804
805 {
806 btVector3 posr = local_origin[0];
807 // float pos[4]={posr.x(),posr.y(),posr.z(),1};
808 btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
809 btTransform tr;
810 tr.setIdentity();
811 tr.setOrigin(posr);
812 tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
813
814 getDebugDrawer()->drawTransform(tr, 0.1);
815
816 }
817
818 for (int k = 0; k<bod->getNumLinks(); k++)
819 {
820 const int parent = bod->getParent(k);
821 world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
822 local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
823 }
824
825
826 for (int m = 0; m<bod->getNumLinks(); m++)
827 {
828 int link = m;
829 int index = link + 1;
830
831 btVector3 posr = local_origin[index];
832 // float pos[4]={posr.x(),posr.y(),posr.z(),1};
833 btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
834 btTransform tr;
835 tr.setIdentity();
836 tr.setOrigin(posr);
837 tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
838
839 getDebugDrawer()->drawTransform(tr, 0.1);
840 }
841 }
842 }
843 }
844
845 btDiscreteDynamicsWorld::debugDrawWorld();
846 }
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
MultiBodyInplaceSolverIslandCallback::operator=
MultiBodyInplaceSolverIslandCallback & operator=(MultiBodyInplaceSolverIslandCallback &other)
Definition: btMultiBodyDynamicsWorld.cpp:256
btMultiBodyDynamicsWorld.h
ACTIVE_TAG
#define ACTIVE_TAG
Definition: btCollisionObject.h:22
MultiBodyInplaceSolverIslandCallback::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btMultiBodyDynamicsWorld.cpp:240
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:54
btMultiBodyDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:124
btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:759
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:276
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
btDiscreteDynamicsWorld::m_islandManager
btSimulationIslandManager * m_islandManager
Definition: btDiscreteDynamicsWorld.h:46
btAlignedObjectArray< btCollisionObject * >
btMultiBody::isAwake
bool isAwake() const
Definition: btMultiBody.h:446
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:145
btDiscreteDynamicsWorld::m_sortedConstraints
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
Definition: btDiscreteDynamicsWorld.h:41
btMultiBody::stepPositionsMultiDof
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
Definition: btMultiBody.cpp:2491
btIDebugDraw::DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:47
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:128
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:176
MultiBodyInplaceSolverIslandCallback::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btMultiBodyDynamicsWorld.cpp:235
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:50
btDiscreteDynamicsWorld
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
Definition: btDiscreteDynamicsWorld.h:37
MultiBodyInplaceSolverIslandCallback::m_numConstraints
int m_numConstraints
Definition: btMultiBodyDynamicsWorld.cpp:234
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMultiBody::getBaseMass
btScalar getBaseMass() const
Definition: btMultiBody.h:148
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btCollisionConfiguration
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size...
Definition: btCollisionConfiguration.h:26
btSimulationIslandManager::buildAndProcessIslands
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
Definition: btSimulationIslandManager.cpp:354
btMultiBodyConstraint.h
btMultiBodyDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btMultiBodyDynamicsWorld.cpp:765
btDiscreteDynamicsWorld::getSimulationIslandManager
btSimulationIslandManager * getSimulationIslandManager()
Definition: btDiscreteDynamicsWorld.h:123
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
btCollisionWorld::getNumCollisionObjects
int getNumCollisionObjects() const
Definition: btCollisionWorld.h:438
btMultiBody::addLinkForce
void addLinkForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:562
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition: btMultiBodyLink.h:132
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:94
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
MultiBodyInplaceSolverIslandCallback::m_solver
btMultiBodyConstraintSolver * m_solver
Definition: btMultiBodyDynamicsWorld.cpp:229
btMultiBodyDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:644
btMultiBody::isPosUpdated
bool isPosUpdated() const
Definition: btMultiBody.h:530
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:117
btMultiBody
Definition: btMultiBody.h:37
btGetMultiBodyConstraintIslandId
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
Definition: btMultiBodyDynamicsWorld.cpp:201
btSimulationIslandManager::updateActivationState
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
Definition: btSimulationIslandManager.cpp:127
btMultiBodyConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)=0
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:367
btSimulationIslandManager::IslandCallback
Definition: btSimulationIslandManager.h:58
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:849
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:542
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
MultiBodyInplaceSolverIslandCallback::m_dispatcher
btDispatcher * m_dispatcher
Definition: btMultiBodyDynamicsWorld.cpp:236
btMultiBodyConstraintSolver::solveGroup
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
this method should not be called, it was just used during porting/integration of Featherstone btMulti...
Definition: btMultiBodyConstraintSolver.cpp:824
btMultiBodyConstraint::getIslandIdB
virtual int getIslandIdB() const =0
btMultiBodyDynamicsWorld::m_solverMultiBodyIslandCallback
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
Definition: btMultiBodyDynamicsWorld.h:36
btDiscreteDynamicsWorld::getCollisionWorld
btCollisionWorld * getCollisionWorld()
Definition: btDiscreteDynamicsWorld.h:133
btMultiBody::getNumPosVars
int getNumPosVars() const
Definition: btMultiBody.h:147
btSortMultiBodyConstraintOnIslandPredicate::operator()
bool operator()(const btMultiBodyConstraint *lhs, const btMultiBodyConstraint *rhs) const
Definition: btMultiBodyDynamicsWorld.cpp:217
MultiBodyInplaceSolverIslandCallback::m_solverInfo
btContactSolverInfo * m_solverInfo
Definition: btMultiBodyDynamicsWorld.cpp:228
MultiBodyInplaceSolverIslandCallback::m_bodies
btAlignedObjectArray< btCollisionObject * > m_bodies
Definition: btMultiBodyDynamicsWorld.cpp:238
btCollisionObject::setActivationState
void setActivationState(int newState) const
Definition: btCollisionObject.cpp:52
MultiBodyInplaceSolverIslandCallback::processIsland
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
Definition: btMultiBodyDynamicsWorld.cpp:281
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btMultiBodyConstraintSolver::solveMultiBodyGroup
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
Definition: btMultiBodyConstraintSolver.cpp:887
btCollisionWorld::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btCollisionWorld.h:102
btCollisionWorld::getDebugDrawer
virtual btIDebugDraw * getDebugDrawer()
Definition: btCollisionWorld.h:162
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld
virtual ~btMultiBodyDynamicsWorld()
Definition: btMultiBodyDynamicsWorld.cpp:392
btDiscreteDynamicsWorld::m_gravity
btVector3 m_gravity
Definition: btDiscreteDynamicsWorld.h:52
btDiscreteDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btDiscreteDynamicsWorld.cpp:283
btMultiBodyDynamicsWorld::removeMultiBodyConstraint
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:754
btSortConstraintOnIslandPredicate2::operator()
bool operator()(const btTypedConstraint *lhs, const btTypedConstraint *rhs) const
Definition: btMultiBodyDynamicsWorld.cpp:190
output
#define output
MultiBodyInplaceSolverIslandCallback::m_manifolds
btAlignedObjectArray< btPersistentManifold * > m_manifolds
Definition: btMultiBodyDynamicsWorld.cpp:239
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:194
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
btMultiBodyConstraintSolver
Definition: btMultiBodyConstraintSolver.h:29
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:28
btMultiBody::getRVector
const btVector3 & getRVector(int i) const
Definition: btMultiBody.cpp:422
MultiBodyInplaceSolverIslandCallback::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.cpp:241
btDiscreteDynamicsWorld::m_predictiveManifolds
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
Definition: btDiscreteDynamicsWorld.h:70
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btBroadphaseInterface
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs...
Definition: btBroadphaseInterface.h:51
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:273
btCollisionObject::setWorldTransform
void setWorldTransform(const btTransform &worldTrans)
Definition: btCollisionObject.h:337
btCollisionWorld::getDispatcher
btDispatcher * getDispatcher()
Definition: btCollisionWorld.h:138
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition: btMultiBodyLink.h:125
btMultiBody::clearVelocities
void clearVelocities()
Definition: btMultiBody.cpp:555
btMultiBodyDynamicsWorld::solveConstraints
virtual void solveConstraints(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:397
MultiBodyInplaceSolverIslandCallback::MultiBodyInplaceSolverIslandCallback
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)
Definition: btMultiBodyDynamicsWorld.cpp:244
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btMultiBody::checkMotionAndSleepIfRequired
void checkMotionAndSleepIfRequired(btScalar timestep)
Definition: btMultiBody.cpp:2861
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:211
btMultiBodyDynamicsWorld::m_multiBodies
btAlignedObjectArray< btMultiBody * > m_multiBodies
Definition: btMultiBodyDynamicsWorld.h:32
btMultiBodyConstraint::getIslandIdA
virtual int getIslandIdA() const =0
btMultiBodyLinkCollider.h
MultiBodyInplaceSolverIslandCallback::m_numMultiBodyConstraints
int m_numMultiBodyConstraints
Definition: btMultiBodyDynamicsWorld.cpp:231
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
MultiBodyInplaceSolverIslandCallback::m_multiBodySortedConstraints
btMultiBodyConstraint ** m_multiBodySortedConstraints
Definition: btMultiBodyDynamicsWorld.cpp:230
btMultiBodyDynamicsWorld::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:33
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
Definition: btMultiBodyDynamicsWorld.cpp:382
btMultiBodyConstraintSolver.h
btSortMultiBodyConstraintOnIslandPredicate
Definition: btMultiBodyDynamicsWorld.cpp:213
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:367
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMultiBody::addBaseForce
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:268
btMultiBody.h
btDiscreteDynamicsWorld::getNumConstraints
virtual int getNumConstraints() const
Definition: btDiscreteDynamicsWorld.cpp:1410
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btConstraintSolver::allSolved
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
Definition: btConstraintSolver.h:52
btContactSolverInfoData::m_minimumSolverBatchSize
int m_minimumSolverBatchSize
Definition: btContactSolverInfo.h:58
btMultiBodyDynamicsWorld::m_sortedMultiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:34
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:398
btIDebugDraw.h
btConstraintSolver::prepareSolve
virtual void prepareSolve(int, int)
Definition: btConstraintSolver.h:47
WANTS_DEACTIVATION
#define WANTS_DEACTIVATION
Definition: btCollisionObject.h:24
btAlignedObjectArray::remove
void remove(const T &key)
Definition: btAlignedObjectArray.h:481
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:137
btMultiBodyDynamicsWorld::addMultiBody
virtual void addMultiBody(btMultiBody *body, short group=btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter)
Definition: btMultiBodyDynamicsWorld.cpp:27
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:77
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:220
btQuickprof.h
btDiscreteDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:977
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:226
btIDebugDraw::getDebugMode
virtual int getDebugMode() const =0
btIDebugDraw::DBG_DrawConstraints
Definition: btIDebugDraw.h:46
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:534
MultiBodyInplaceSolverIslandCallback::setup
void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
Definition: btMultiBodyDynamicsWorld.cpp:263
btSimulationIslandManager::storeIslandActivationState
virtual void storeIslandActivationState(btCollisionWorld *world)
Definition: btSimulationIslandManager.cpp:152
btMultiBody::getParentToLocalRot
const btQuaternion & getParentToLocalRot(int i) const
Definition: btMultiBody.cpp:427
DISABLE_DEACTIVATION
#define DISABLE_DEACTIVATION
Definition: btCollisionObject.h:25
btMultiBodyDynamicsWorld::removeMultiBody
virtual void removeMultiBody(btMultiBody *body)
Definition: btMultiBodyDynamicsWorld.cpp:33
btSimulationIslandManager::getUnionFind
btUnionFind & getUnionFind()
Definition: btSimulationIslandManager.h:48
btMultiBody::isUsingRK4Integration
bool isUsingRK4Integration() const
Definition: btMultiBody.h:526
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:29
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btDiscreteDynamicsWorld::m_constraintSolver
btConstraintSolver * m_constraintSolver
Definition: btDiscreteDynamicsWorld.h:44
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:124
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:522
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
btMultiBody::stepVelocitiesMultiDof
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
Definition: btMultiBody.cpp:1078
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:171
btDiscreteDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:621
MultiBodyInplaceSolverIslandCallback::processConstraints
void processConstraints()
Definition: btMultiBodyDynamicsWorld.cpp:361
btMultiBody::stepPositions
void stepPositions(btScalar dt)
Definition: btMultiBody.cpp:2455
btMultiBody::getParent
int getParent(int link_num) const
Definition: btMultiBody.cpp:362
btUnionFind::unite
void unite(int p, int q)
Definition: btUnionFind.h:81
MultiBodyInplaceSolverIslandCallback::m_sortedConstraints
btTypedConstraint ** m_sortedConstraints
Definition: btMultiBodyDynamicsWorld.cpp:233
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:166
btMultiBodyDynamicsWorld::addMultiBodyConstraint
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:749
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
btCollisionObject::getActivationState
int getActivationState() const
Definition: btCollisionObject.h:269
btSimulationIslandManager.h
btDynamicsWorld::getSolverInfo
btContactSolverInfo & getSolverInfo()
Definition: btDynamicsWorld.h:132
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:230
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:146
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:95
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:224
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:365
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:56
btGetConstraintIslandId2
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
Definition: btMultiBodyDynamicsWorld.cpp:174
btMultiBody::stepVelocities
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
Definition: btMultiBody.cpp:1552
btMultiBodyDynamicsWorld::calculateSimulationIslands
virtual void calculateSimulationIslands()
Definition: btMultiBodyDynamicsWorld.cpp:38
MultiBodyInplaceSolverIslandCallback
Definition: btMultiBodyDynamicsWorld.cpp:226
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btDiscreteDynamicsWorld::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btDiscreteDynamicsWorld.h:48
btSortConstraintOnIslandPredicate2
Definition: btMultiBodyDynamicsWorld.cpp:186