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 #include "LinearMath/btSerializer.h"
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::forwardKinematics()
398 {
399 btAlignedObjectArray<btQuaternion> world_to_local;
400 btAlignedObjectArray<btVector3> local_origin;
401
402 for (int b=0;b<m_multiBodies.size();b++)
403 {
404 btMultiBody* bod = m_multiBodies[b];
405 bod->forwardKinematics(world_to_local,local_origin);
406 }
407 }
408 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
409 {
410 forwardKinematics();
411
412 btAlignedObjectArray<btScalar> scratch_r;
413 btAlignedObjectArray<btVector3> scratch_v;
414 btAlignedObjectArray<btMatrix3x3> scratch_m;
415
416
417 BT_PROFILE("solveConstraints");
418
419 m_sortedConstraints.resize( m_constraints.size());
420 int i;
421 for (i=0;i<getNumConstraints();i++)
422 {
423 m_sortedConstraints[i] = m_constraints[i];
424 }
425 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
426 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
427
428 m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
429 for (i=0;i<m_multiBodyConstraints.size();i++)
430 {
431 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
432 }
433 m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
434
435 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
436
437
438 m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
439 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
440
442 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
443
444 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
445 {
446 BT_PROFILE("btMultiBody addForce");
447 for (int i=0;i<this->m_multiBodies.size();i++)
448 {
449 btMultiBody* bod = m_multiBodies[i];
450
451 bool isSleeping = false;
452
453 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
454 {
455 isSleeping = true;
456 }
457 for (int b=0;b<bod->getNumLinks();b++)
458 {
459 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
460 isSleeping = true;
461 }
462
463 if (!isSleeping)
464 {
465 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
466 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
467 scratch_v.resize(bod->getNumLinks()+1);
468 scratch_m.resize(bod->getNumLinks()+1);
469
470 bod->addBaseForce(m_gravity * bod->getBaseMass());
471
472 for (int j = 0; j < bod->getNumLinks(); ++j)
473 {
474 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
475 }
476 }//if (!isSleeping)
477 }
478 }
479 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
480
481
482 {
483 BT_PROFILE("btMultiBody stepVelocities");
484 for (int i=0;i<this->m_multiBodies.size();i++)
485 {
486 btMultiBody* bod = m_multiBodies[i];
487
488 bool isSleeping = false;
489
490 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
491 {
492 isSleeping = true;
493 }
494 for (int b=0;b<bod->getNumLinks();b++)
495 {
496 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
497 isSleeping = true;
498 }
499
500 if (!isSleeping)
501 {
502 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
503 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
504 scratch_v.resize(bod->getNumLinks()+1);
505 scratch_m.resize(bod->getNumLinks()+1);
506 bool doNotUpdatePos = false;
507
508 if(bod->isMultiDof())
509 {
510 if(!bod->isUsingRK4Integration())
511 {
512 bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
513 }
514 else
515 {
516 //
517 int numDofs = bod->getNumDofs() + 6;
518 int numPosVars = bod->getNumPosVars() + 7;
519 btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
520 //convenience
521 btScalar *pMem = &scratch_r2[0];
522 btScalar *scratch_q0 = pMem; pMem += numPosVars;
523 btScalar *scratch_qx = pMem; pMem += numPosVars;
524 btScalar *scratch_qd0 = pMem; pMem += numDofs;
525 btScalar *scratch_qd1 = pMem; pMem += numDofs;
526 btScalar *scratch_qd2 = pMem; pMem += numDofs;
527 btScalar *scratch_qd3 = pMem; pMem += numDofs;
528 btScalar *scratch_qdd0 = pMem; pMem += numDofs;
529 btScalar *scratch_qdd1 = pMem; pMem += numDofs;
530 btScalar *scratch_qdd2 = pMem; pMem += numDofs;
531 btScalar *scratch_qdd3 = pMem; pMem += numDofs;
532 btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
533
535 //copy q0 to scratch_q0 and qd0 to scratch_qd0
536 scratch_q0[0] = bod->getWorldToBaseRot().x();
537 scratch_q0[1] = bod->getWorldToBaseRot().y();
538 scratch_q0[2] = bod->getWorldToBaseRot().z();
539 scratch_q0[3] = bod->getWorldToBaseRot().w();
540 scratch_q0[4] = bod->getBasePos().x();
541 scratch_q0[5] = bod->getBasePos().y();
542 scratch_q0[6] = bod->getBasePos().z();
543 //
544 for(int link = 0; link < bod->getNumLinks(); ++link)
545 {
546 for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
547 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
548 }
549 //
550 for(int dof = 0; dof < numDofs; ++dof)
551 scratch_qd0[dof] = bod->getVelocityVector()[dof];
553 struct
554 {
555 btMultiBody *bod;
556 btScalar *scratch_qx, *scratch_q0;
557
558 void operator()()
559 {
560 for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
561 scratch_qx[dof] = scratch_q0[dof];
562 }
563 } pResetQx = {bod, scratch_qx, scratch_q0};
564 //
565 struct
566 {
567 void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
568 {
569 for(int i = 0; i < size; ++i)
570 pVal[i] = pCurVal[i] + dt * pDer[i];
571 }
572
573 } pEulerIntegrate;
574 //
575 struct
576 {
577 void operator()(btMultiBody *pBody, const btScalar *pData)
578 {
579 btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
580
581 for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
582 pVel[i] = pData[i];
583
584 }
585 } pCopyToVelocityVector;
586 //
587 struct
588 {
589 void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
590 {
591 for(int i = 0; i < size; ++i)
592 pDst[i] = pSrc[start + i];
593 }
594 } pCopy;
595 //
596
597 btScalar h = solverInfo.m_timeStep;
598 #define output &scratch_r[bod->getNumDofs()]
599 //calc qdd0 from: q0 & qd0
600 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
601 pCopy(output, scratch_qdd0, 0, numDofs);
602 //calc q1 = q0 + h/2 * qd0
603 pResetQx();
604 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
605 //calc qd1 = qd0 + h/2 * qdd0
606 pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
607 //
608 //calc qdd1 from: q1 & qd1
609 pCopyToVelocityVector(bod, scratch_qd1);
610 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
611 pCopy(output, scratch_qdd1, 0, numDofs);
612 //calc q2 = q0 + h/2 * qd1
613 pResetQx();
614 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
615 //calc qd2 = qd0 + h/2 * qdd1
616 pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
617 //
618 //calc qdd2 from: q2 & qd2
619 pCopyToVelocityVector(bod, scratch_qd2);
620 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
621 pCopy(output, scratch_qdd2, 0, numDofs);
622 //calc q3 = q0 + h * qd2
623 pResetQx();
624 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
625 //calc qd3 = qd0 + h * qdd2
626 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
627 //
628 //calc qdd3 from: q3 & qd3
629 pCopyToVelocityVector(bod, scratch_qd3);
630 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
631 pCopy(output, scratch_qdd3, 0, numDofs);
632
633 //
634 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
635 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
636 btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
637 btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
638 for(int i = 0; i < numDofs; ++i)
639 {
640 delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
641 delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
642 //delta_q[i] = h*scratch_qd0[i];
643 //delta_qd[i] = h*scratch_qdd0[i];
644 }
645 //
646 pCopyToVelocityVector(bod, scratch_qd0);
647 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
648 //
649 if(!doNotUpdatePos)
650 {
651 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
652 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
653
654 for(int i = 0; i < numDofs; ++i)
655 pRealBuf[i] = delta_q[i];
656
657 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
658 bod->setPosUpdated(true);
659 }
660
661 //ugly hack which resets the cached data to t0 (needed for constraint solver)
662 {
663 for(int link = 0; link < bod->getNumLinks(); ++link)
664 bod->getLink(link).updateCacheMultiDof();
665 bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
666 }
667
668 }
669 }
670 else//if(bod->isMultiDof())
671 {
672 bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
673 }
674 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
675 bod->clearForcesAndTorques();
676 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
677 }//if (!isSleeping)
678 }
679 }
680
681 clearMultiBodyConstraintForces();
682
683 m_solverMultiBodyIslandCallback->processConstraints();
684
685 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
686
687 {
688 BT_PROFILE("btMultiBody stepVelocities");
689 for (int i=0;i<this->m_multiBodies.size();i++)
690 {
691 btMultiBody* bod = m_multiBodies[i];
692
693 bool isSleeping = false;
694
695 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
696 {
697 isSleeping = true;
698 }
699 for (int b=0;b<bod->getNumLinks();b++)
700 {
701 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
702 isSleeping = true;
703 }
704
705 if (!isSleeping)
706 {
707 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
708 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
709 scratch_v.resize(bod->getNumLinks()+1);
710 scratch_m.resize(bod->getNumLinks()+1);
711
712 if(bod->isMultiDof())
713 {
714 if(!bod->isUsingRK4Integration())
715 {
716 bool isConstraintPass = true;
717 bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
718 }
719 }
720 }
721 }
722 }
723
724 for (int i=0;i<this->m_multiBodies.size();i++)
725 {
726 btMultiBody* bod = m_multiBodies[i];
727 bod->processDeltaVeeMultiDof2();
728 }
729
730 }
731
732 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
733 {
734 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
735
736 {
737 BT_PROFILE("btMultiBody stepPositions");
738 //integrate and update the Featherstone hierarchies
739 btAlignedObjectArray<btQuaternion> world_to_local;
740 btAlignedObjectArray<btVector3> local_origin;
741
742 for (int b=0;b<m_multiBodies.size();b++)
743 {
744 btMultiBody* bod = m_multiBodies[b];
745 bool isSleeping = false;
746 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
747 {
748 isSleeping = true;
749 }
750 for (int b=0;b<bod->getNumLinks();b++)
751 {
752 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
753 isSleeping = true;
754 }
755
756
757 if (!isSleeping)
758 {
759 int nLinks = bod->getNumLinks();
760
762
763 if(bod->isMultiDof())
764 {
765 if(!bod->isPosUpdated())
766 bod->stepPositionsMultiDof(timeStep);
767 else
768 {
769 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
770 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
771
772 bod->stepPositionsMultiDof(1, 0, pRealBuf);
773 bod->setPosUpdated(false);
774 }
775 }
776 else
777 {
778 bod->stepPositions(timeStep);
779 }
780 world_to_local.resize(nLinks+1);
781 local_origin.resize(nLinks+1);
782
783 bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
784
785 } else
786 {
787 bod->clearVelocities();
788 }
789 }
790 }
791 }
792
793
794
795 void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
796 {
797 m_multiBodyConstraints.push_back(constraint);
798 }
799
800 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
801 {
802 m_multiBodyConstraints.remove(constraint);
803 }
804
805 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
806 {
807 constraint->debugDraw(getDebugDrawer());
808 }
809
810
811 void btMultiBodyDynamicsWorld::debugDrawWorld()
812 {
813 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
814
815 bool drawConstraints = false;
816 if (getDebugDrawer())
817 {
818 int mode = getDebugDrawer()->getDebugMode();
819 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
820 {
821 drawConstraints = true;
822 }
823
824 if (drawConstraints)
825 {
826 BT_PROFILE("btMultiBody debugDrawWorld");
827
828 btAlignedObjectArray<btQuaternion> world_to_local1;
829 btAlignedObjectArray<btVector3> local_origin1;
830
831 for (int c=0;c<m_multiBodyConstraints.size();c++)
832 {
833 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
834 debugDrawMultiBodyConstraint(constraint);
835 }
836
837 for (int b = 0; b<m_multiBodies.size(); b++)
838 {
839 btMultiBody* bod = m_multiBodies[b];
840 bod->forwardKinematics(world_to_local1,local_origin1);
841
842 getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
843
844
845 for (int m = 0; m<bod->getNumLinks(); m++)
846 {
847
848 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
849
850 getDebugDrawer()->drawTransform(tr, 0.1);
851
852 //draw the joint axis
853 if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
854 {
855 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
856
857 btVector4 color(0,0,0,1);//1,1,1);
858 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
859 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
860 getDebugDrawer()->drawLine(from,to,color);
861 }
862 if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
863 {
864 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
865
866 btVector4 color(0,0,0,1);//1,1,1);
867 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
868 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
869 getDebugDrawer()->drawLine(from,to,color);
870 }
871 if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
872 {
873 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
874
875 btVector4 color(0,0,0,1);//1,1,1);
876 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
877 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
878 getDebugDrawer()->drawLine(from,to,color);
879 }
880
881 }
882 }
883 }
884 }
885
886 btDiscreteDynamicsWorld::debugDrawWorld();
887 }
888
889
890
891 void btMultiBodyDynamicsWorld::applyGravity()
892 {
893 btDiscreteDynamicsWorld::applyGravity();
894 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
895 BT_PROFILE("btMultiBody addGravity");
896 for (int i=0;i<this->m_multiBodies.size();i++)
897 {
898 btMultiBody* bod = m_multiBodies[i];
899
900 bool isSleeping = false;
901
902 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
903 {
904 isSleeping = true;
905 }
906 for (int b=0;b<bod->getNumLinks();b++)
907 {
908 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
909 isSleeping = true;
910 }
911
912 if (!isSleeping)
913 {
914 bod->addBaseForce(m_gravity * bod->getBaseMass());
915
916 for (int j = 0; j < bod->getNumLinks(); ++j)
917 {
918 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
919 }
920 }//if (!isSleeping)
921 }
922 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
923 }
924
925 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
926 {
927 for (int i=0;i<this->m_multiBodies.size();i++)
928 {
929 btMultiBody* bod = m_multiBodies[i];
930 bod->clearConstraintForces();
931 }
932 }
933 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
934 {
935 {
936 BT_PROFILE("clearMultiBodyForces");
937 for (int i=0;i<this->m_multiBodies.size();i++)
938 {
939 btMultiBody* bod = m_multiBodies[i];
940
941 bool isSleeping = false;
942
943 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
944 {
945 isSleeping = true;
946 }
947 for (int b=0;b<bod->getNumLinks();b++)
948 {
949 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
950 isSleeping = true;
951 }
952
953 if (!isSleeping)
954 {
955 btMultiBody* bod = m_multiBodies[i];
956 bod->clearForcesAndTorques();
957 }
958 }
959 }
960
961 }
962 void btMultiBodyDynamicsWorld::clearForces()
963 {
964 btDiscreteDynamicsWorld::clearForces();
965
966 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
967 clearMultiBodyForces();
968 #endif
969 }
970
971
972
973
974 void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
975 {
976
977 serializer->startSerialization();
978
979 serializeDynamicsWorldInfo( serializer);
980
981 serializeMultiBodies(serializer);
982
983 serializeRigidBodies(serializer);
984
985 serializeCollisionObjects(serializer);
986
987 serializer->finishSerialization();
988 }
989
990 void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
991 {
992 int i;
993 //serialize all collision objects
994 for (i=0;i<m_multiBodies.size();i++)
995 {
996 btMultiBody* mb = m_multiBodies[i];
997 {
998 int len = mb->calculateSerializeBufferSize();
999 btChunk* chunk = serializer->allocate(len,1);
1000 const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
1001 serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
1002 }
1003 }
1004
1005 }
MultiBodyInplaceSolverIslandCallback::operator=
MultiBodyInplaceSolverIslandCallback & operator=(MultiBodyInplaceSolverIslandCallback &other)
Definition: btMultiBodyDynamicsWorld.cpp:256
btSerializer::finishSerialization
virtual void finishSerialization()=0
btDiscreteDynamicsWorld::serializeDynamicsWorldInfo
void serializeDynamicsWorldInfo(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1454
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
btMultiBody::stepVelocitiesMultiDof
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
Definition: btMultiBody.cpp:672
btCollisionWorld::serializeCollisionObjects
void serializeCollisionObjects(btSerializer *serializer)
Definition: btCollisionWorld.cpp:1520
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:805
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:559
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:268
btChunk
Definition: btSerializer.h:51
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
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:974
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btDiscreteDynamicsWorld::m_islandManager
btSimulationIslandManager * m_islandManager
Definition: btDiscreteDynamicsWorld.h:46
btAlignedObjectArray< btCollisionObject * >
btMultiBody::isAwake
bool isAwake() const
Definition: btMultiBody.h:503
BT_MULTIBODY_CODE
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:117
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:154
btMultiBodyDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep...
Definition: btMultiBodyDynamicsWorld.cpp:962
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:73
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:1978
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:127
btSerializer::startSerialization
virtual void startSerialization()=0
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:121
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:199
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:182
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
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:2481
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:157
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:811
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:590
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition: btMultiBodyLink.h:125
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:732
btMultiBody::isPosUpdated
bool isPosUpdated() const
Definition: btMultiBody.h:587
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:104
btMultiBody
Definition: btMultiBody.h:48
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:2428
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:422
btSimulationIslandManager::IslandCallback
Definition: btSimulationIslandManager.h:58
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition: btMultiBodyLink.h:59
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btSerializer
Definition: btSerializer.h:68
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:570
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:884
btMultibodyLink::eFixed
Definition: btMultiBodyLink.h:77
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:864
btMultiBodyConstraint::getIslandIdB
virtual int getIslandIdB() const =0
btMultiBodyDynamicsWorld::m_solverMultiBodyIslandCallback
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
Definition: btMultiBodyDynamicsWorld.h:37
btDiscreteDynamicsWorld::getCollisionWorld
btCollisionWorld * getCollisionWorld()
Definition: btDiscreteDynamicsWorld.h:133
btMultiBody::getNumPosVars
int getNumPosVars() const
Definition: btMultiBody.h:156
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:2382
btVector4
Definition: btVector3.h:1077
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
btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces
virtual void clearMultiBodyConstraintForces()
Definition: btMultiBodyDynamicsWorld.cpp:925
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
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:1135
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:800
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:96
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
btMultiBodyConstraintSolver
Definition: btMultiBodyConstraintSolver.h:30
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
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
btMultiBodyDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btMultiBodyDynamicsWorld.cpp:891
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:51
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:273
btCollisionWorld::getDispatcher
btDispatcher * getDispatcher()
Definition: btCollisionWorld.h:138
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition: btMultiBodyLink.h:115
btMultiBody::clearVelocities
void clearVelocities()
Definition: btMultiBody.cpp:583
btMultiBodyDynamicsWorld::solveConstraints
virtual void solveConstraints(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:408
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:2344
btDiscreteDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btDiscreteDynamicsWorld.cpp:339
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:211
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
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
btMultiBodyConstraintSolver.h
btSortMultiBodyConstraintOnIslandPredicate
Definition: btMultiBodyDynamicsWorld.cpp:213
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:374
btMultiBodyDynamicsWorld::serializeMultiBodies
virtual void serializeMultiBodies(btSerializer *serializer)
Definition: btMultiBodyDynamicsWorld.cpp:990
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:294
btMultiBody.h
btDiscreteDynamicsWorld::getNumConstraints
virtual int getNumConstraints() const
Definition: btDiscreteDynamicsWorld.cpp:1410
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
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:35
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:473
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
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:212
btQuickprof.h
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:70
btDiscreteDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:977
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:226
btMultiBodyDynamicsWorld::clearMultiBodyForces
virtual void clearMultiBodyForces()
Definition: btMultiBodyDynamicsWorld.cpp:933
btIDebugDraw::getDebugMode
virtual int getDebugMode() const =0
btIDebugDraw::DBG_DrawConstraints
Definition: btIDebugDraw.h:70
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:591
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
btMultiBody::isUsingRK4Integration
bool isUsingRK4Integration() const
Definition: btMultiBody.h:583
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:131
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:133
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:579
btDiscreteDynamicsWorld::serializeRigidBodies
void serializeRigidBodies(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1425
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:181
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:1942
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:176
btMultiBodyDynamicsWorld::addMultiBodyConstraint
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:795
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:74
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:56
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:155
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:95
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
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:2488
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:248
btMultiBody::processDeltaVeeMultiDof2
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:412
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:357
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:1215
btMultiBodyDynamicsWorld::calculateSimulationIslands
virtual void calculateSimulationIslands()
Definition: btMultiBodyDynamicsWorld.cpp:38
MultiBodyInplaceSolverIslandCallback
Definition: btMultiBodyDynamicsWorld.cpp:226
btSerializer.h
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