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

Bullet Collision Detection & Physics Library

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 #include "LinearMath/btSerializer.h"
25
26
27 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int 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::forwardKinematics()
398 {
399
400 for (int b=0;b<m_multiBodies.size();b++)
401 {
402 btMultiBody* bod = m_multiBodies[b];
403 bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin);
404 }
405 }
406 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
407 {
408 forwardKinematics();
409
410
411
412 BT_PROFILE("solveConstraints");
413
414 m_sortedConstraints.resize( m_constraints.size());
415 int i;
416 for (i=0;i<getNumConstraints();i++)
417 {
418 m_sortedConstraints[i] = m_constraints[i];
419 }
420 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
421 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
422
423 m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
424 for (i=0;i<m_multiBodyConstraints.size();i++)
425 {
426 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
427 }
428 m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
429
430 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
431
432
433 m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
434 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
435
437 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
438
439 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
440 {
441 BT_PROFILE("btMultiBody addForce");
442 for (int i=0;i<this->m_multiBodies.size();i++)
443 {
444 btMultiBody* bod = m_multiBodies[i];
445
446 bool isSleeping = false;
447
448 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
449 {
450 isSleeping = true;
451 }
452 for (int b=0;b<bod->getNumLinks();b++)
453 {
454 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
455 isSleeping = true;
456 }
457
458 if (!isSleeping)
459 {
460 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
461 m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
462 m_scratch_v.resize(bod->getNumLinks()+1);
463 m_scratch_m.resize(bod->getNumLinks()+1);
464
465 bod->addBaseForce(m_gravity * bod->getBaseMass());
466
467 for (int j = 0; j < bod->getNumLinks(); ++j)
468 {
469 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
470 }
471 }//if (!isSleeping)
472 }
473 }
474 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
475
476
477 {
478 BT_PROFILE("btMultiBody stepVelocities");
479 for (int i=0;i<this->m_multiBodies.size();i++)
480 {
481 btMultiBody* bod = m_multiBodies[i];
482
483 bool isSleeping = false;
484
485 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
486 {
487 isSleeping = true;
488 }
489 for (int b=0;b<bod->getNumLinks();b++)
490 {
491 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
492 isSleeping = true;
493 }
494
495 if (!isSleeping)
496 {
497 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
498 m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
499 m_scratch_v.resize(bod->getNumLinks()+1);
500 m_scratch_m.resize(bod->getNumLinks()+1);
501 bool doNotUpdatePos = false;
502
503 {
504 if(!bod->isUsingRK4Integration())
505 {
506 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
507 }
508 else
509 {
510 //
511 int numDofs = bod->getNumDofs() + 6;
512 int numPosVars = bod->getNumPosVars() + 7;
513 btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
514 //convenience
515 btScalar *pMem = &scratch_r2[0];
516 btScalar *scratch_q0 = pMem; pMem += numPosVars;
517 btScalar *scratch_qx = pMem; pMem += numPosVars;
518 btScalar *scratch_qd0 = pMem; pMem += numDofs;
519 btScalar *scratch_qd1 = pMem; pMem += numDofs;
520 btScalar *scratch_qd2 = pMem; pMem += numDofs;
521 btScalar *scratch_qd3 = pMem; pMem += numDofs;
522 btScalar *scratch_qdd0 = pMem; pMem += numDofs;
523 btScalar *scratch_qdd1 = pMem; pMem += numDofs;
524 btScalar *scratch_qdd2 = pMem; pMem += numDofs;
525 btScalar *scratch_qdd3 = pMem; pMem += numDofs;
526 btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
527
529 //copy q0 to scratch_q0 and qd0 to scratch_qd0
530 scratch_q0[0] = bod->getWorldToBaseRot().x();
531 scratch_q0[1] = bod->getWorldToBaseRot().y();
532 scratch_q0[2] = bod->getWorldToBaseRot().z();
533 scratch_q0[3] = bod->getWorldToBaseRot().w();
534 scratch_q0[4] = bod->getBasePos().x();
535 scratch_q0[5] = bod->getBasePos().y();
536 scratch_q0[6] = bod->getBasePos().z();
537 //
538 for(int link = 0; link < bod->getNumLinks(); ++link)
539 {
540 for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
541 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
542 }
543 //
544 for(int dof = 0; dof < numDofs; ++dof)
545 scratch_qd0[dof] = bod->getVelocityVector()[dof];
547 struct
548 {
549 btMultiBody *bod;
550 btScalar *scratch_qx, *scratch_q0;
551
552 void operator()()
553 {
554 for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
555 scratch_qx[dof] = scratch_q0[dof];
556 }
557 } pResetQx = {bod, scratch_qx, scratch_q0};
558 //
559 struct
560 {
561 void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
562 {
563 for(int i = 0; i < size; ++i)
564 pVal[i] = pCurVal[i] + dt * pDer[i];
565 }
566
567 } pEulerIntegrate;
568 //
569 struct
570 {
571 void operator()(btMultiBody *pBody, const btScalar *pData)
572 {
573 btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
574
575 for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
576 pVel[i] = pData[i];
577
578 }
579 } pCopyToVelocityVector;
580 //
581 struct
582 {
583 void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
584 {
585 for(int i = 0; i < size; ++i)
586 pDst[i] = pSrc[start + i];
587 }
588 } pCopy;
589 //
590
591 btScalar h = solverInfo.m_timeStep;
592 #define output &m_scratch_r[bod->getNumDofs()]
593 //calc qdd0 from: q0 & qd0
594 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
595 pCopy(output, scratch_qdd0, 0, numDofs);
596 //calc q1 = q0 + h/2 * qd0
597 pResetQx();
598 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
599 //calc qd1 = qd0 + h/2 * qdd0
600 pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
601 //
602 //calc qdd1 from: q1 & qd1
603 pCopyToVelocityVector(bod, scratch_qd1);
604 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
605 pCopy(output, scratch_qdd1, 0, numDofs);
606 //calc q2 = q0 + h/2 * qd1
607 pResetQx();
608 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
609 //calc qd2 = qd0 + h/2 * qdd1
610 pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
611 //
612 //calc qdd2 from: q2 & qd2
613 pCopyToVelocityVector(bod, scratch_qd2);
614 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
615 pCopy(output, scratch_qdd2, 0, numDofs);
616 //calc q3 = q0 + h * qd2
617 pResetQx();
618 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
619 //calc qd3 = qd0 + h * qdd2
620 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
621 //
622 //calc qdd3 from: q3 & qd3
623 pCopyToVelocityVector(bod, scratch_qd3);
624 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
625 pCopy(output, scratch_qdd3, 0, numDofs);
626
627 //
628 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
629 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
630 btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
631 btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
632 for(int i = 0; i < numDofs; ++i)
633 {
634 delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
635 delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
636 //delta_q[i] = h*scratch_qd0[i];
637 //delta_qd[i] = h*scratch_qdd0[i];
638 }
639 //
640 pCopyToVelocityVector(bod, scratch_qd0);
641 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
642 //
643 if(!doNotUpdatePos)
644 {
645 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
646 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
647
648 for(int i = 0; i < numDofs; ++i)
649 pRealBuf[i] = delta_q[i];
650
651 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
652 bod->setPosUpdated(true);
653 }
654
655 //ugly hack which resets the cached data to t0 (needed for constraint solver)
656 {
657 for(int link = 0; link < bod->getNumLinks(); ++link)
658 bod->getLink(link).updateCacheMultiDof();
659 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
660 }
661
662 }
663 }
664
665 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
666 bod->clearForcesAndTorques();
667 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
668 }//if (!isSleeping)
669 }
670 }
671
672 clearMultiBodyConstraintForces();
673
674 m_solverMultiBodyIslandCallback->processConstraints();
675
676 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
677
678 {
679 BT_PROFILE("btMultiBody stepVelocities");
680 for (int i=0;i<this->m_multiBodies.size();i++)
681 {
682 btMultiBody* bod = m_multiBodies[i];
683
684 bool isSleeping = false;
685
686 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
687 {
688 isSleeping = true;
689 }
690 for (int b=0;b<bod->getNumLinks();b++)
691 {
692 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
693 isSleeping = true;
694 }
695
696 if (!isSleeping)
697 {
698 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
699 m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
700 m_scratch_v.resize(bod->getNumLinks()+1);
701 m_scratch_m.resize(bod->getNumLinks()+1);
702
703
704 {
705 if(!bod->isUsingRK4Integration())
706 {
707 bool isConstraintPass = true;
708 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
709 }
710 }
711 }
712 }
713 }
714
715 for (int i=0;i<this->m_multiBodies.size();i++)
716 {
717 btMultiBody* bod = m_multiBodies[i];
718 bod->processDeltaVeeMultiDof2();
719 }
720
721 }
722
723 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
724 {
725 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
726
727 {
728 BT_PROFILE("btMultiBody stepPositions");
729 //integrate and update the Featherstone hierarchies
730
731 for (int b=0;b<m_multiBodies.size();b++)
732 {
733 btMultiBody* bod = m_multiBodies[b];
734 bool isSleeping = false;
735 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
736 {
737 isSleeping = true;
738 }
739 for (int b=0;b<bod->getNumLinks();b++)
740 {
741 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
742 isSleeping = true;
743 }
744
745
746 if (!isSleeping)
747 {
748 int nLinks = bod->getNumLinks();
749
751
752
753 {
754 if(!bod->isPosUpdated())
755 bod->stepPositionsMultiDof(timeStep);
756 else
757 {
758 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
759 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
760
761 bod->stepPositionsMultiDof(1, 0, pRealBuf);
762 bod->setPosUpdated(false);
763 }
764 }
765
766 m_scratch_world_to_local.resize(nLinks+1);
767 m_scratch_local_origin.resize(nLinks+1);
768
769 bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin);
770
771 } else
772 {
773 bod->clearVelocities();
774 }
775 }
776 }
777 }
778
779
780
781 void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
782 {
783 m_multiBodyConstraints.push_back(constraint);
784 }
785
786 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
787 {
788 m_multiBodyConstraints.remove(constraint);
789 }
790
791 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
792 {
793 constraint->debugDraw(getDebugDrawer());
794 }
795
796
797 void btMultiBodyDynamicsWorld::debugDrawWorld()
798 {
799 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
800
801 btDiscreteDynamicsWorld::debugDrawWorld();
802
803 bool drawConstraints = false;
804 if (getDebugDrawer())
805 {
806 int mode = getDebugDrawer()->getDebugMode();
807 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
808 {
809 drawConstraints = true;
810 }
811
812 if (drawConstraints)
813 {
814 BT_PROFILE("btMultiBody debugDrawWorld");
815
816
817 for (int c=0;c<m_multiBodyConstraints.size();c++)
818 {
819 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
820 debugDrawMultiBodyConstraint(constraint);
821 }
822
823 for (int b = 0; b<m_multiBodies.size(); b++)
824 {
825 btMultiBody* bod = m_multiBodies[b];
826 bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
827
828 getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
829
830
831 for (int m = 0; m<bod->getNumLinks(); m++)
832 {
833
834 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
835
836 getDebugDrawer()->drawTransform(tr, 0.1);
837
838 //draw the joint axis
839 if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
840 {
841 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
842
843 btVector4 color(0,0,0,1);//1,1,1);
844 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
845 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
846 getDebugDrawer()->drawLine(from,to,color);
847 }
848 if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
849 {
850 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
851
852 btVector4 color(0,0,0,1);//1,1,1);
853 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
854 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
855 getDebugDrawer()->drawLine(from,to,color);
856 }
857 if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
858 {
859 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
860
861 btVector4 color(0,0,0,1);//1,1,1);
862 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
863 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
864 getDebugDrawer()->drawLine(from,to,color);
865 }
866
867 }
868 }
869 }
870 }
871
872
873 }
874
875
876
877 void btMultiBodyDynamicsWorld::applyGravity()
878 {
879 btDiscreteDynamicsWorld::applyGravity();
880 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
881 BT_PROFILE("btMultiBody addGravity");
882 for (int i=0;i<this->m_multiBodies.size();i++)
883 {
884 btMultiBody* bod = m_multiBodies[i];
885
886 bool isSleeping = false;
887
888 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
889 {
890 isSleeping = true;
891 }
892 for (int b=0;b<bod->getNumLinks();b++)
893 {
894 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
895 isSleeping = true;
896 }
897
898 if (!isSleeping)
899 {
900 bod->addBaseForce(m_gravity * bod->getBaseMass());
901
902 for (int j = 0; j < bod->getNumLinks(); ++j)
903 {
904 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
905 }
906 }//if (!isSleeping)
907 }
908 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
909 }
910
911 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
912 {
913 for (int i=0;i<this->m_multiBodies.size();i++)
914 {
915 btMultiBody* bod = m_multiBodies[i];
916 bod->clearConstraintForces();
917 }
918 }
919 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
920 {
921 {
922 // BT_PROFILE("clearMultiBodyForces");
923 for (int i=0;i<this->m_multiBodies.size();i++)
924 {
925 btMultiBody* bod = m_multiBodies[i];
926
927 bool isSleeping = false;
928
929 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
930 {
931 isSleeping = true;
932 }
933 for (int b=0;b<bod->getNumLinks();b++)
934 {
935 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
936 isSleeping = true;
937 }
938
939 if (!isSleeping)
940 {
941 btMultiBody* bod = m_multiBodies[i];
942 bod->clearForcesAndTorques();
943 }
944 }
945 }
946
947 }
948 void btMultiBodyDynamicsWorld::clearForces()
949 {
950 btDiscreteDynamicsWorld::clearForces();
951
952 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
953 clearMultiBodyForces();
954 #endif
955 }
956
957
958
959
960 void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
961 {
962
963 serializer->startSerialization();
964
965 serializeDynamicsWorldInfo( serializer);
966
967 serializeMultiBodies(serializer);
968
969 serializeRigidBodies(serializer);
970
971 serializeCollisionObjects(serializer);
972
973 serializer->finishSerialization();
974 }
975
976 void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
977 {
978 int i;
979 //serialize all collision objects
980 for (i=0;i<m_multiBodies.size();i++)
981 {
982 btMultiBody* mb = m_multiBodies[i];
983 {
984 int len = mb->calculateSerializeBufferSize();
985 btChunk* chunk = serializer->allocate(len,1);
986 const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
987 serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
988 }
989 }
990
991 }
MultiBodyInplaceSolverIslandCallback::operator=
MultiBodyInplaceSolverIslandCallback & operator=(MultiBodyInplaceSolverIslandCallback &other)
Definition: btMultiBodyDynamicsWorld.cpp:256
btSerializer::finishSerialization
virtual void finishSerialization()=0
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btDiscreteDynamicsWorld::serializeDynamicsWorldInfo
void serializeDynamicsWorldInfo(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1472
btMultiBodyDynamicsWorld.h
ACTIVE_TAG
#define ACTIVE_TAG
Definition: btCollisionObject.h:22
MultiBodyInplaceSolverIslandCallback::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btMultiBodyDynamicsWorld.cpp:240
btDiscreteDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep...
Definition: btDiscreteDynamicsWorld.cpp:326
btCollisionWorld::serializeCollisionObjects
void serializeCollisionObjects(btSerializer *serializer)
Definition: btCollisionWorld.cpp:1609
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btMultiBodyDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:124
btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:791
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:597
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:274
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:186
btChunk
Definition: btSerializer.h:51
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:164
btMultiBodyDynamicsWorld::serialize
virtual void serialize(btSerializer *serializer)
Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see B...
Definition: btMultiBodyDynamicsWorld.cpp:960
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btDiscreteDynamicsWorld::m_islandManager
btSimulationIslandManager * m_islandManager
Definition: btDiscreteDynamicsWorld.h:47
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:53
BT_MULTIBODY_CODE
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:117
btMultiBodyDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep...
Definition: btMultiBodyDynamicsWorld.cpp:948
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:74
btDiscreteDynamicsWorld::m_sortedConstraints
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
Definition: btDiscreteDynamicsWorld.h:42
btMultiBody::stepPositionsMultiDof
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
Definition: btMultiBody.cpp:1542
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btIDebugDraw::DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:71
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:137
btSerializer::startSerialization
virtual void startSerialization()=0
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:131
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:193
MultiBodyInplaceSolverIslandCallback::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btMultiBodyDynamicsWorld.cpp:235
btDiscreteDynamicsWorld
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
Definition: btDiscreteDynamicsWorld.h:38
btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
Definition: btMultiBody.cpp:710
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:222
btMultiBodyDynamicsWorld::addMultiBody
virtual void addMultiBody(btMultiBody *body, int group=btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter)
Definition: btMultiBodyDynamicsWorld.cpp:27
MultiBodyInplaceSolverIslandCallback::m_numConstraints
int m_numConstraints
Definition: btMultiBodyDynamicsWorld.cpp:234
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMultiBodyDynamicsWorld::m_scratch_r
btAlignedObjectArray< btScalar > m_scratch_r
Definition: btMultiBodyDynamicsWorld.h:44
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
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:797
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:191
btDiscreteDynamicsWorld::getSimulationIslandManager
btSimulationIslandManager * getSimulationIslandManager()
Definition: btDiscreteDynamicsWorld.h:128
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
btMultiBody::addLinkForce
void addLinkForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:628
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition: btMultiBodyLink.h:135
btMultiBody::isPosUpdated
bool isPosUpdated() const
Definition: btMultiBody.h:556
btMultiBody::isUsingRK4Integration
bool isUsingRK4Integration() const
Definition: btMultiBody.h:552
btCollisionObject::getActivationState
int getActivationState() const
Definition: btCollisionObject.h:282
MultiBodyInplaceSolverIslandCallback::m_solver
btMultiBodyConstraintSolver * m_solver
Definition: btMultiBodyDynamicsWorld.cpp:229
btMultiBodyDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:723
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:114
btMultiBody
Definition: btMultiBody.h:51
btGetMultiBodyConstraintIslandId
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
Definition: btMultiBodyDynamicsWorld.cpp:201
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1893
btSimulationIslandManager::updateActivationState
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
Definition: btSimulationIslandManager.cpp:127
btMultiBodyDynamicsWorld::m_scratch_local_origin
btAlignedObjectArray< btVector3 > m_scratch_local_origin
Definition: btMultiBodyDynamicsWorld.h:41
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:226
btMultiBodyConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)=0
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
btSimulationIslandManager::IslandCallback
Definition: btSimulationIslandManager.h:58
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:207
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition: btMultiBodyLink.h:60
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btDynamicsWorld::m_solverInfo
btContactSolverInfo m_solverInfo
Definition: btDynamicsWorld.h:50
btSerializer
Definition: btSerializer.h:68
btMultiBodyDynamicsWorld::m_multiBodyConstraintSolver
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
Definition: btMultiBodyDynamicsWorld.h:36
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:608
btCollisionWorld::getNumCollisionObjects
int getNumCollisionObjects() const
Definition: btCollisionWorld.h:440
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
btMultibodyLink::eFixed
Definition: btMultiBodyLink.h:78
MultiBodyInplaceSolverIslandCallback::m_dispatcher
btDispatcher * m_dispatcher
Definition: btMultiBodyDynamicsWorld.cpp:236
btMultiBodyConstraint::getIslandIdB
virtual int getIslandIdB() const =0
btMultiBodyDynamicsWorld::m_solverMultiBodyIslandCallback
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
Definition: btMultiBodyDynamicsWorld.h:37
btDiscreteDynamicsWorld::getCollisionWorld
btCollisionWorld * getCollisionWorld()
Definition: btDiscreteDynamicsWorld.h:138
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1847
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:1946
btVector4
Definition: btVector3.h:1089
MultiBodyInplaceSolverIslandCallback::m_solverInfo
btContactSolverInfo * m_solverInfo
Definition: btMultiBodyDynamicsWorld.cpp:228
MultiBodyInplaceSolverIslandCallback::m_bodies
btAlignedObjectArray< btCollisionObject * > m_bodies
Definition: btMultiBodyDynamicsWorld.cpp:238
btMultiBodyDynamicsWorld::m_scratch_v
btAlignedObjectArray< btVector3 > m_scratch_v
Definition: btMultiBodyDynamicsWorld.h:45
btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces
virtual void clearMultiBodyConstraintForces()
Definition: btMultiBodyDynamicsWorld.cpp:911
MultiBodyInplaceSolverIslandCallback::processIsland
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
Definition: btMultiBodyDynamicsWorld.cpp:281
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
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:1417
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:209
btCollisionWorld::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btCollisionWorld.h:102
btMultiBody::isAwake
bool isAwake() const
Definition: btMultiBody.h:472
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btCollisionWorld::getDebugDrawer
virtual btIDebugDraw * getDebugDrawer()
Definition: btCollisionWorld.h:162
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld
virtual ~btMultiBodyDynamicsWorld()
Definition: btMultiBodyDynamicsWorld.cpp:392
btMultiBody::getBaseMass
btScalar getBaseMass() const
Definition: btMultiBody.h:167
btDiscreteDynamicsWorld::m_gravity
btVector3 m_gravity
Definition: btDiscreteDynamicsWorld.h:53
btDiscreteDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btDiscreteDynamicsWorld.cpp:283
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:102
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:207
btMultiBodyDynamicsWorld::removeMultiBodyConstraint
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:786
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:97
output
#define output
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:165
MultiBodyInplaceSolverIslandCallback::m_manifolds
btAlignedObjectArray< btPersistentManifold * > m_manifolds
Definition: btMultiBodyDynamicsWorld.cpp:239
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btMultiBodyConstraintSolver
Definition: btMultiBodyConstraintSolver.h:30
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btMultiBody::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btMultiBody.cpp:1953
MultiBodyInplaceSolverIslandCallback::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.cpp:241
btDiscreteDynamicsWorld::m_predictiveManifolds
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
Definition: btDiscreteDynamicsWorld.h:71
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btMultiBodyDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btMultiBodyDynamicsWorld.cpp:877
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:70
btBroadphaseInterface
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs...
Definition: btBroadphaseInterface.h:55
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:286
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:621
btMultiBodyDynamicsWorld::solveConstraints
virtual void solveConstraints(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:406
MultiBodyInplaceSolverIslandCallback::MultiBodyInplaceSolverIslandCallback
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)
Definition: btMultiBodyDynamicsWorld.cpp:244
btMultiBody::checkMotionAndSleepIfRequired
void checkMotionAndSleepIfRequired(btScalar timestep)
Definition: btMultiBody.cpp:1816
btDiscreteDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btDiscreteDynamicsWorld.cpp:339
btMultiBodyDynamicsWorld::m_multiBodies
btAlignedObjectArray< btMultiBody * > m_multiBodies
Definition: btMultiBodyDynamicsWorld.h:33
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
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
MultiBodyInplaceSolverIslandCallback::m_multiBodySortedConstraints
btMultiBodyConstraint ** m_multiBodySortedConstraints
Definition: btMultiBodyDynamicsWorld.cpp:230
btMultiBodyDynamicsWorld::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:34
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
Definition: btMultiBodyDynamicsWorld.cpp:382
btMultiBody::getNumPosVars
int getNumPosVars() const
Definition: btMultiBody.h:166
btMultiBodyConstraintSolver.h
btSortMultiBodyConstraintOnIslandPredicate
Definition: btMultiBodyDynamicsWorld.cpp:213
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btMultiBodyDynamicsWorld::serializeMultiBodies
virtual void serializeMultiBodies(btSerializer *serializer)
Definition: btMultiBodyDynamicsWorld.cpp:976
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
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:308
btMultiBody.h
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btContactSolverInfo
Definition: btContactSolverInfo.h:69
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:61
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:443
btMultiBodyDynamicsWorld::m_sortedMultiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:35
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:505
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:78
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
btQuickprof.h
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:70
btDiscreteDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:1095
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:103
btMultiBodyDynamicsWorld::clearMultiBodyForces
virtual void clearMultiBodyForces()
Definition: btMultiBodyDynamicsWorld.cpp:919
btIDebugDraw::getDebugMode
virtual int getDebugMode() const =0
btIDebugDraw::DBG_DrawConstraints
Definition: btIDebugDraw.h:70
btMultiBodyDynamicsWorld::m_scratch_world_to_local1
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local1
Definition: btMultiBodyDynamicsWorld.h:42
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:560
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
DISABLE_DEACTIVATION
#define DISABLE_DEACTIVATION
Definition: btCollisionObject.h:25
btMultiBodyDynamicsWorld::removeMultiBody
virtual void removeMultiBody(btMultiBody *body)
Definition: btMultiBodyDynamicsWorld.cpp:33
btMultiBodyDynamicsWorld::forwardKinematics
void forwardKinematics()
Definition: btMultiBodyDynamicsWorld.cpp:397
btSimulationIslandManager::getUnionFind
btUnionFind & getUnionFind()
Definition: btSimulationIslandManager.h:48
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btMultiBodyDynamicsWorld::m_scratch_m
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
Definition: btMultiBodyDynamicsWorld.h:46
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btDiscreteDynamicsWorld::m_constraintSolver
btConstraintSolver * m_constraintSolver
Definition: btDiscreteDynamicsWorld.h:45
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:357
btMultiBodyDynamicsWorld::m_scratch_world_to_local
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local
Definition: btMultiBodyDynamicsWorld.h:40
btDiscreteDynamicsWorld::getNumConstraints
virtual int getNumConstraints() const
Definition: btDiscreteDynamicsWorld.cpp:1428
btDiscreteDynamicsWorld::serializeRigidBodies
void serializeRigidBodies(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1443
btDiscreteDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:620
MultiBodyInplaceSolverIslandCallback::processConstraints
void processConstraints()
Definition: btMultiBodyDynamicsWorld.cpp:361
btMultiBodyDynamicsWorld::m_scratch_local_origin1
btAlignedObjectArray< btVector3 > m_scratch_local_origin1
Definition: btMultiBodyDynamicsWorld.h:43
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
btMultiBodyDynamicsWorld::addMultiBodyConstraint
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:781
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:75
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:56
btSimulationIslandManager.h
btDynamicsWorld::getSolverInfo
btContactSolverInfo & getSolverInfo()
Definition: btDynamicsWorld.h:133
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:258
btCollisionObject::setActivationState
void setActivationState(int newState) const
Definition: btCollisionObject.cpp:60
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btMultiBody::processDeltaVeeMultiDof2
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:390
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:363
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:59
btGetConstraintIslandId2
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
Definition: btMultiBodyDynamicsWorld.cpp:174
btMultiBodyDynamicsWorld::calculateSimulationIslands
virtual void calculateSimulationIslands()
Definition: btMultiBodyDynamicsWorld.cpp:38
MultiBodyInplaceSolverIslandCallback
Definition: btMultiBodyDynamicsWorld.cpp:226
btSerializer.h
btDiscreteDynamicsWorld::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btDiscreteDynamicsWorld.h:49
btSortConstraintOnIslandPredicate2
Definition: btMultiBodyDynamicsWorld.cpp:186