Bullet Collision Detection & Physics Library
btMultiBodyDynamicsWorld.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
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  {
353  processConstraints();
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  {
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 
678 
680 
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 
692  {
693  isSleeping = true;
694  }
695  for (int b=0;b<bod->getNumLinks();b++)
696  {
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];
724  }
725 
726 }
727 
729 {
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;
743  {
744  isSleeping = true;
745  }
746  for (int b=0;b<bod->getNumLinks();b++)
747  {
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 
789 {
790  m_multiBodyConstraints.push_back(constraint);
791 }
792 
794 {
795  m_multiBodyConstraints.remove(constraint);
796 }
797 
799 {
800  constraint->debugDraw(getDebugDrawer());
801 }
802 
803 
805 {
806  BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
807 
808  bool drawConstraints = false;
809  if (getDebugDrawer())
810  {
811  int mode = getDebugDrawer()->getDebugMode();
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  {
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 
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
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  }
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  }
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 
880 }
881 
882 
883 
885 {
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 
896  {
897  isSleeping = true;
898  }
899  for (int b=0;b<bod->getNumLinks();b++)
900  {
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 
919 {
920  for (int i=0;i<this->m_multiBodies.size();i++)
921  {
922  btMultiBody* bod = m_multiBodies[i];
923  bod->clearConstraintForces();
924  }
925 }
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 
937  {
938  isSleeping = true;
939  }
940  for (int b=0;b<bod->getNumLinks();b++)
941  {
943  isSleeping = true;
944  }
945 
946  if (!isSleeping)
947  {
948  btMultiBody* bod = m_multiBodies[i];
949  bod->clearForcesAndTorques();
950  }
951  }
952  }
953 
954 }
956 {
958 
959 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
961 #endif
962 }
963 
964 
965 
966 
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 
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 &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 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:119
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:459
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:117
int getNumLinks() const
Definition: btMultiBody.h:155
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:200
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
virtual int calculateSerializeBufferSize() const
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btScalar getBaseMass() const
Definition: btMultiBody.h:158
#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:543
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:387
#define ISLAND_SLEEPING
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btContactSolverInfo m_solverInfo
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
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...
virtual int getIslandIdB() const =0
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
btCollisionWorld * getCollisionWorld()
int getNumPosVars() const
Definition: btMultiBody.h:157
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
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)
#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
virtual int getIslandIdA() const =0
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:295
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
virtual int getDebugMode() const =0
void setPosUpdated(bool updated)
Definition: btMultiBody.h:547
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:539
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btConstraintSolver * m_constraintSolver
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
void serializeRigidBodies(btSerializer *serializer)
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:182
virtual void updateActivationState(btScalar timeStep)
void unite(int p, int q)
Definition: btUnionFind.h:81
const btVector3 & getBasePos() const
Definition: btMultiBody.h:177
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:156
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:249
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:377
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)
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray< btTypedConstraint * > m_constraints