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 {
509 if(!bod->isUsingRK4Integration())
510 {
511 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
512 }
513 else
514 {
515 //
516 int numDofs = bod->getNumDofs() + 6;
517 int numPosVars = bod->getNumPosVars() + 7;
518 btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
519 //convenience
520 btScalar *pMem = &scratch_r2[0];
521 btScalar *scratch_q0 = pMem; pMem += numPosVars;
522 btScalar *scratch_qx = pMem; pMem += numPosVars;
523 btScalar *scratch_qd0 = pMem; pMem += numDofs;
524 btScalar *scratch_qd1 = pMem; pMem += numDofs;
525 btScalar *scratch_qd2 = pMem; pMem += numDofs;
526 btScalar *scratch_qd3 = pMem; pMem += numDofs;
527 btScalar *scratch_qdd0 = pMem; pMem += numDofs;
528 btScalar *scratch_qdd1 = pMem; pMem += numDofs;
529 btScalar *scratch_qdd2 = pMem; pMem += numDofs;
530 btScalar *scratch_qdd3 = pMem; pMem += numDofs;
531 btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
532
534 //copy q0 to scratch_q0 and qd0 to scratch_qd0
535 scratch_q0[0] = bod->getWorldToBaseRot().x();
536 scratch_q0[1] = bod->getWorldToBaseRot().y();
537 scratch_q0[2] = bod->getWorldToBaseRot().z();
538 scratch_q0[3] = bod->getWorldToBaseRot().w();
539 scratch_q0[4] = bod->getBasePos().x();
540 scratch_q0[5] = bod->getBasePos().y();
541 scratch_q0[6] = bod->getBasePos().z();
542 //
543 for(int link = 0; link < bod->getNumLinks(); ++link)
544 {
545 for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
546 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
547 }
548 //
549 for(int dof = 0; dof < numDofs; ++dof)
550 scratch_qd0[dof] = bod->getVelocityVector()[dof];
552 struct
553 {
554 btMultiBody *bod;
555 btScalar *scratch_qx, *scratch_q0;
556
557 void operator()()
558 {
559 for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
560 scratch_qx[dof] = scratch_q0[dof];
561 }
562 } pResetQx = {bod, scratch_qx, scratch_q0};
563 //
564 struct
565 {
566 void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
567 {
568 for(int i = 0; i < size; ++i)
569 pVal[i] = pCurVal[i] + dt * pDer[i];
570 }
571
572 } pEulerIntegrate;
573 //
574 struct
575 {
576 void operator()(btMultiBody *pBody, const btScalar *pData)
577 {
578 btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
579
580 for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
581 pVel[i] = pData[i];
582
583 }
584 } pCopyToVelocityVector;
585 //
586 struct
587 {
588 void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
589 {
590 for(int i = 0; i < size; ++i)
591 pDst[i] = pSrc[start + i];
592 }
593 } pCopy;
594 //
595
596 btScalar h = solverInfo.m_timeStep;
597 #define output &scratch_r[bod->getNumDofs()]
598 //calc qdd0 from: q0 & qd0
599 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
600 pCopy(output, scratch_qdd0, 0, numDofs);
601 //calc q1 = q0 + h/2 * qd0
602 pResetQx();
603 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
604 //calc qd1 = qd0 + h/2 * qdd0
605 pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
606 //
607 //calc qdd1 from: q1 & qd1
608 pCopyToVelocityVector(bod, scratch_qd1);
609 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
610 pCopy(output, scratch_qdd1, 0, numDofs);
611 //calc q2 = q0 + h/2 * qd1
612 pResetQx();
613 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
614 //calc qd2 = qd0 + h/2 * qdd1
615 pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
616 //
617 //calc qdd2 from: q2 & qd2
618 pCopyToVelocityVector(bod, scratch_qd2);
619 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
620 pCopy(output, scratch_qdd2, 0, numDofs);
621 //calc q3 = q0 + h * qd2
622 pResetQx();
623 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
624 //calc qd3 = qd0 + h * qdd2
625 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
626 //
627 //calc qdd3 from: q3 & qd3
628 pCopyToVelocityVector(bod, scratch_qd3);
629 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
630 pCopy(output, scratch_qdd3, 0, numDofs);
631
632 //
633 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
634 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
635 btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
636 btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
637 for(int i = 0; i < numDofs; ++i)
638 {
639 delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
640 delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
641 //delta_q[i] = h*scratch_qd0[i];
642 //delta_qd[i] = h*scratch_qdd0[i];
643 }
644 //
645 pCopyToVelocityVector(bod, scratch_qd0);
646 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
647 //
648 if(!doNotUpdatePos)
649 {
650 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
651 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
652
653 for(int i = 0; i < numDofs; ++i)
654 pRealBuf[i] = delta_q[i];
655
656 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
657 bod->setPosUpdated(true);
658 }
659
660 //ugly hack which resets the cached data to t0 (needed for constraint solver)
661 {
662 for(int link = 0; link < bod->getNumLinks(); ++link)
663 bod->getLink(link).updateCacheMultiDof();
664 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
665 }
666
667 }
668 }
669
670 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
671 bod->clearForcesAndTorques();
672 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
673 }//if (!isSleeping)
674 }
675 }
676
677 clearMultiBodyConstraintForces();
678
679 m_solverMultiBodyIslandCallback->processConstraints();
680
681 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
682
683 {
684 BT_PROFILE("btMultiBody stepVelocities");
685 for (int i=0;i<this->m_multiBodies.size();i++)
686 {
687 btMultiBody* bod = m_multiBodies[i];
688
689 bool isSleeping = false;
690
691 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
692 {
693 isSleeping = true;
694 }
695 for (int b=0;b<bod->getNumLinks();b++)
696 {
697 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
698 isSleeping = true;
699 }
700
701 if (!isSleeping)
702 {
703 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
704 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
705 scratch_v.resize(bod->getNumLinks()+1);
706 scratch_m.resize(bod->getNumLinks()+1);
707
708
709 {
710 if(!bod->isUsingRK4Integration())
711 {
712 bool isConstraintPass = true;
713 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
714 }
715 }
716 }
717 }
718 }
719
720 for (int i=0;i<this->m_multiBodies.size();i++)
721 {
722 btMultiBody* bod = m_multiBodies[i];
723 bod->processDeltaVeeMultiDof2();
724 }
725
726 }
727
728 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
729 {
730 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
731
732 {
733 BT_PROFILE("btMultiBody stepPositions");
734 //integrate and update the Featherstone hierarchies
735 btAlignedObjectArray<btQuaternion> world_to_local;
736 btAlignedObjectArray<btVector3> local_origin;
737
738 for (int b=0;b<m_multiBodies.size();b++)
739 {
740 btMultiBody* bod = m_multiBodies[b];
741 bool isSleeping = false;
742 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
743 {
744 isSleeping = true;
745 }
746 for (int b=0;b<bod->getNumLinks();b++)
747 {
748 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
749 isSleeping = true;
750 }
751
752
753 if (!isSleeping)
754 {
755 int nLinks = bod->getNumLinks();
756
758
759
760 {
761 if(!bod->isPosUpdated())
762 bod->stepPositionsMultiDof(timeStep);
763 else
764 {
765 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
766 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
767
768 bod->stepPositionsMultiDof(1, 0, pRealBuf);
769 bod->setPosUpdated(false);
770 }
771 }
772
773 world_to_local.resize(nLinks+1);
774 local_origin.resize(nLinks+1);
775
776 bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
777
778 } else
779 {
780 bod->clearVelocities();
781 }
782 }
783 }
784 }
785
786
787
788 void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
789 {
790 m_multiBodyConstraints.push_back(constraint);
791 }
792
793 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
794 {
795 m_multiBodyConstraints.remove(constraint);
796 }
797
798 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
799 {
800 constraint->debugDraw(getDebugDrawer());
801 }
802
803
804 void btMultiBodyDynamicsWorld::debugDrawWorld()
805 {
806 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
807
808 bool drawConstraints = false;
809 if (getDebugDrawer())
810 {
811 int mode = getDebugDrawer()->getDebugMode();
812 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
813 {
814 drawConstraints = true;
815 }
816
817 if (drawConstraints)
818 {
819 BT_PROFILE("btMultiBody debugDrawWorld");
820
821 btAlignedObjectArray<btQuaternion> world_to_local1;
822 btAlignedObjectArray<btVector3> local_origin1;
823
824 for (int c=0;c<m_multiBodyConstraints.size();c++)
825 {
826 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
827 debugDrawMultiBodyConstraint(constraint);
828 }
829
830 for (int b = 0; b<m_multiBodies.size(); b++)
831 {
832 btMultiBody* bod = m_multiBodies[b];
833 bod->forwardKinematics(world_to_local1,local_origin1);
834
835 getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
836
837
838 for (int m = 0; m<bod->getNumLinks(); m++)
839 {
840
841 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
842
843 getDebugDrawer()->drawTransform(tr, 0.1);
844
845 //draw the joint axis
846 if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
847 {
848 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
849
850 btVector4 color(0,0,0,1);//1,1,1);
851 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
852 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
853 getDebugDrawer()->drawLine(from,to,color);
854 }
855 if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
856 {
857 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
858
859 btVector4 color(0,0,0,1);//1,1,1);
860 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
861 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
862 getDebugDrawer()->drawLine(from,to,color);
863 }
864 if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
865 {
866 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
867
868 btVector4 color(0,0,0,1);//1,1,1);
869 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
870 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
871 getDebugDrawer()->drawLine(from,to,color);
872 }
873
874 }
875 }
876 }
877 }
878
879 btDiscreteDynamicsWorld::debugDrawWorld();
880 }
881
882
883
884 void btMultiBodyDynamicsWorld::applyGravity()
885 {
886 btDiscreteDynamicsWorld::applyGravity();
887 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
888 BT_PROFILE("btMultiBody addGravity");
889 for (int i=0;i<this->m_multiBodies.size();i++)
890 {
891 btMultiBody* bod = m_multiBodies[i];
892
893 bool isSleeping = false;
894
895 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
896 {
897 isSleeping = true;
898 }
899 for (int b=0;b<bod->getNumLinks();b++)
900 {
901 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
902 isSleeping = true;
903 }
904
905 if (!isSleeping)
906 {
907 bod->addBaseForce(m_gravity * bod->getBaseMass());
908
909 for (int j = 0; j < bod->getNumLinks(); ++j)
910 {
911 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
912 }
913 }//if (!isSleeping)
914 }
915 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
916 }
917
918 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
919 {
920 for (int i=0;i<this->m_multiBodies.size();i++)
921 {
922 btMultiBody* bod = m_multiBodies[i];
923 bod->clearConstraintForces();
924 }
925 }
926 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
927 {
928 {
929 BT_PROFILE("clearMultiBodyForces");
930 for (int i=0;i<this->m_multiBodies.size();i++)
931 {
932 btMultiBody* bod = m_multiBodies[i];
933
934 bool isSleeping = false;
935
936 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
937 {
938 isSleeping = true;
939 }
940 for (int b=0;b<bod->getNumLinks();b++)
941 {
942 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
943 isSleeping = true;
944 }
945
946 if (!isSleeping)
947 {
948 btMultiBody* bod = m_multiBodies[i];
949 bod->clearForcesAndTorques();
950 }
951 }
952 }
953
954 }
955 void btMultiBodyDynamicsWorld::clearForces()
956 {
957 btDiscreteDynamicsWorld::clearForces();
958
959 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
960 clearMultiBodyForces();
961 #endif
962 }
963
964
965
966
967 void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
968 {
969
970 serializer->startSerialization();
971
972 serializeDynamicsWorldInfo( serializer);
973
974 serializeMultiBodies(serializer);
975
976 serializeRigidBodies(serializer);
977
978 serializeCollisionObjects(serializer);
979
980 serializer->finishSerialization();
981 }
982
983 void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
984 {
985 int i;
986 //serialize all collision objects
987 for (i=0;i<m_multiBodies.size();i++)
988 {
989 btMultiBody* mb = m_multiBodies[i];
990 {
991 int len = mb->calculateSerializeBufferSize();
992 btChunk* chunk = serializer->allocate(len,1);
993 const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
994 serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
995 }
996 }
997
998 }
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
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:798
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:530
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:274
btChunk
Definition: btSerializer.h:51
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
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:967
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:459
BT_MULTIBODY_CODE
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:117
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:155
btMultiBodyDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep...
Definition: btMultiBodyDynamicsWorld.cpp:955
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:1459
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:200
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:168
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::computeAccelerationsArticulatedBodyAlgorithmMultiDof
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
Definition: btMultiBody.cpp:643
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:1864
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:158
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:804
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:561
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:728
btMultiBody::isPosUpdated
bool isPosUpdated() const
Definition: btMultiBody.h:543
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:104
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:1811
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:387
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
btDynamicsWorld::m_solverInfo
btContactSolverInfo m_solverInfo
Definition: btDynamicsWorld.h:49
btSerializer
Definition: btSerializer.h:68
btMultiBodyDynamicsWorld::m_multiBodyConstraintSolver
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
Definition: btMultiBodyDynamicsWorld.h:36
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:541
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:786
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:157
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1765
btVector4
Definition: btVector3.h:1077
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:918
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:155
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:1051
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:793
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:96
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:884
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:554
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:1733
btDiscreteDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btDiscreteDynamicsWorld.cpp:339
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:212
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:345
btMultiBodyDynamicsWorld::serializeMultiBodies
virtual void serializeMultiBodies(btSerializer *serializer)
Definition: btMultiBodyDynamicsWorld.cpp:983
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:295
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:479
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: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:977
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:227
btMultiBodyDynamicsWorld::clearMultiBodyForces
virtual void clearMultiBodyForces()
Definition: btMultiBodyDynamicsWorld.cpp:926
btIDebugDraw::getDebugMode
virtual int getDebugMode() const =0
btIDebugDraw::DBG_DrawConstraints
Definition: btIDebugDraw.h:70
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:547
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:539
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:134
btDiscreteDynamicsWorld::serializeRigidBodies
void serializeRigidBodies(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1425
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:182
btDiscreteDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:621
MultiBodyInplaceSolverIslandCallback::processConstraints
void processConstraints()
Definition: btMultiBodyDynamicsWorld.cpp:361
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:177
btMultiBodyDynamicsWorld::addMultiBodyConstraint
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:788
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:231
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:156
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:1871
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:249
btMultiBody::processDeltaVeeMultiDof2
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:377
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:363
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:56
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
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