Bullet
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 
18 #include "btMultiBody.h"
21 #include "LinearMath/btQuickprof.h"
22 #include "btMultiBodyConstraint.h"
25 
26 
27 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
28 {
30 
31 }
32 
34 {
35  m_multiBodies.remove(body);
36 }
37 
39 {
40  BT_PROFILE("calculateSimulationIslands");
41 
43 
44  {
45  //merge islands based on speculative contact manifolds too
46  for (int i=0;i<this->m_predictiveManifolds.size();i++)
47  {
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  {
87 
88  for (int b=0;b<body->getNumLinks();b++)
89  {
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  {
111  int tagA = c->getIslandIdA();
112  int tagB = c->getIslandIdB();
113  if (tagA>=0 && tagB>=0)
115  }
116  }
117 
118  //Store the island id in each body
120 
121 }
122 
123 
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  {
139  if (col && col->getActivationState() == ACTIVE_TAG)
140  {
142  col->setDeactivationTime(0.f);
143  }
144  for (int b=0;b<body->getNumLinks();b++)
145  {
147  if (col && col->getActivationState() == ACTIVE_TAG)
148  {
150  col->setDeactivationTime(0.f);
151  }
152  }
153  } else
154  {
156  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
158 
159  for (int b=0;b<body->getNumLinks();b++)
160  {
162  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
164  }
165  }
166 
167  }
168  }
169 
171 }
172 
173 
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 
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 
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 
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 
227 {
232 
237 
242 
243 
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 
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  {
354  } else
355  {
356  //printf("deferred\n");
357  }
358  }
359  }
360  }
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 
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;
390 }
391 
393 {
395 }
396 
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 }
409 {
411 
415 
416 
417  BT_PROFILE("solveConstraints");
418 
420  int i;
421  for (i=0;i<getNumConstraints();i++)
422  {
424  }
426  btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
427 
429  for (i=0;i<m_multiBodyConstraints.size();i++)
430  {
432  }
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());
440 
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 
454  {
455  isSleeping = true;
456  }
457  for (int b=0;b<bod->getNumLinks();b++)
458  {
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 
491  {
492  isSleeping = true;
493  }
494  for (int b=0;b<bod->getNumLinks();b++)
495  {
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 
682 
684 
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 
696  {
697  isSleeping = true;
698  }
699  for (int b=0;b<bod->getNumLinks();b++)
700  {
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];
728  }
729 
730 }
731 
733 {
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;
747  {
748  isSleeping = true;
749  }
750  for (int b=0;b<bod->getNumLinks();b++)
751  {
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 
796 {
797  m_multiBodyConstraints.push_back(constraint);
798 }
799 
801 {
802  m_multiBodyConstraints.remove(constraint);
803 }
804 
806 {
807  constraint->debugDraw(getDebugDrawer());
808 }
809 
810 
812 {
813  BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
814 
815  bool drawConstraints = false;
816  if (getDebugDrawer())
817  {
818  int mode = getDebugDrawer()->getDebugMode();
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  {
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 
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
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  }
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  }
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 
887 }
888 
889 
890 
892 {
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 
903  {
904  isSleeping = true;
905  }
906  for (int b=0;b<bod->getNumLinks();b++)
907  {
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 
926 {
927  for (int i=0;i<this->m_multiBodies.size();i++)
928  {
929  btMultiBody* bod = m_multiBodies[i];
930  bod->clearConstraintForces();
931  }
932 }
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 
944  {
945  isSleeping = true;
946  }
947  for (int b=0;b<bod->getNumLinks();b++)
948  {
950  isSleeping = true;
951  }
952 
953  if (!isSleeping)
954  {
955  btMultiBody* bod = m_multiBodies[i];
956  bod->clearForcesAndTorques();
957  }
958  }
959  }
960 
961 }
963 {
965 
966 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
968 #endif
969 }
970 
971 
972 
973 
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 
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 &other)
virtual void finishSerialization()=0
void serializeDynamicsWorldInfo(btSerializer *serializer)
#define ACTIVE_TAG
btAlignedObjectArray< btTypedConstraint * > m_constraints
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep...
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
void serializeCollisionObjects(btSerializer *serializer)
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
virtual void updateActivationState(btScalar timeStep)
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
void clearConstraintForces()
void push_back(const T &_Val)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
virtual void serialize(btSerializer *serializer)
Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see B...
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btSimulationIslandManager * m_islandManager
bool isAwake() const
Definition: btMultiBody.h:503
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:117
int getNumLinks() const
Definition: btMultiBody.h:154
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep...
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual void startSerialization()=0
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:199
virtual int getDebugMode() const =0
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
virtual int calculateSerializeBufferSize() const
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btScalar getBaseMass() const
Definition: btMultiBody.h:157
#define btAssert(x)
Definition: btScalar.h:113
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size...
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
btSimulationIslandManager * getSimulationIslandManager()
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
int getNumCollisionObjects() const
void addLinkForce(int i, const btVector3 &f)
const btCollisionObject * getBody0() const
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
virtual void integrateTransforms(btScalar timeStep)
bool isPosUpdated() const
Definition: btMultiBody.h:587
virtual int getIslandIdB() const =0
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
virtual void debugDraw(class btIDebugDraw *drawer)=0
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:422
#define ISLAND_SLEEPING
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
void clearForcesAndTorques()
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
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...
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
btCollisionWorld * getCollisionWorld()
int getNumPosVars() const
Definition: btMultiBody.h:156
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
bool operator()(const btMultiBodyConstraint *lhs, const btMultiBodyConstraint *rhs) const
btAlignedObjectArray< btCollisionObject * > m_bodies
void setActivationState(int newState) const
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
int size() const
return the number of elements in the array
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
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)
btIDebugDraw * m_debugDrawer
virtual btIDebugDraw * getDebugDrawer()
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
bool operator()(const btTypedConstraint *lhs, const btTypedConstraint *rhs) const
virtual int getIslandIdA() const =0
#define output
btAlignedObjectArray< btPersistentManifold * > m_manifolds
bool isStaticOrKinematicObject() const
btCollisionObject can be used to manage collision detection objects.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
virtual void applyGravity()
apply gravity, call this once per timestep
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs...
void setDeactivationTime(btScalar time)
btDispatcher * getDispatcher()
void clearVelocities()
virtual void solveConstraints(btContactSolverInfo &solverInfo)
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
void checkMotionAndSleepIfRequired(btScalar timestep)
virtual void applyGravity()
apply gravity, call this once per timestep
bool isEnabled() const
btAlignedObjectArray< btMultiBody * > m_multiBodies
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
btScalar getLinkMass(int i) const
virtual void serializeMultiBodies(btSerializer *serializer)
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:294
virtual int getNumConstraints() const
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
int getIslandTag() const
virtual void prepareSolve(int, int)
#define WANTS_DEACTIVATION
void remove(const T &key)
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
virtual void addMultiBody(btMultiBody *body, short group=btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
virtual void integrateTransforms(btScalar timeStep)
const btRigidBody & getRigidBodyA() const
void setPosUpdated(bool updated)
Definition: btMultiBody.h:591
void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
virtual void storeIslandActivationState(btCollisionWorld *world)
#define DISABLE_DEACTIVATION
virtual void removeMultiBody(btMultiBody *body)
bool isUsingRK4Integration() const
Definition: btMultiBody.h:583
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btConstraintSolver * m_constraintSolver
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:133
bool isMultiDof()
Definition: btMultiBody.h:579
void serializeRigidBodies(btSerializer *serializer)
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:181
virtual void updateActivationState(btScalar timeStep)
void stepPositions(btScalar dt)
void unite(int p, int q)
Definition: btUnionFind.h:81
const btVector3 & getBasePos() const
Definition: btMultiBody.h:176
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
void * m_oldPtr
Definition: btSerializer.h:56
int getActivationState() const
btContactSolverInfo & getSolverInfo()
const btRigidBody & getRigidBodyB() const
int getNumDofs() const
Definition: btMultiBody.h:155
const btCollisionObject * getBody1() const
virtual btChunk * allocate(size_t size, int numElements)=0
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:248
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:412
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
void quickSort(const L &CompareFunc)
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray< btTypedConstraint * > m_constraints