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