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 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
27 {
29 }
30 
32 {
33  m_multiBodies.remove(body);
34 }
35 
37 {
38  BT_PROFILE("calculateSimulationIslands");
39 
41 
42  {
43  //merge islands based on speculative contact manifolds too
44  for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
45  {
47 
48  const btCollisionObject* colObj0 = manifold->getBody0();
49  const btCollisionObject* colObj1 = manifold->getBody1();
50 
51  if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
52  ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
53  {
54  getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
55  }
56  }
57  }
58 
59  {
60  int i;
61  int numConstraints = int(m_constraints.size());
62  for (i = 0; i < numConstraints; i++)
63  {
64  btTypedConstraint* constraint = m_constraints[i];
65  if (constraint->isEnabled())
66  {
67  const btRigidBody* colObj0 = &constraint->getRigidBodyA();
68  const btRigidBody* colObj1 = &constraint->getRigidBodyB();
69 
70  if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
71  ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
72  {
73  getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
74  }
75  }
76  }
77  }
78 
79  //merge islands linked by Featherstone link colliders
80  for (int i = 0; i < m_multiBodies.size(); i++)
81  {
82  btMultiBody* body = m_multiBodies[i];
83  {
85 
86  for (int b = 0; b < body->getNumLinks(); b++)
87  {
89 
90  if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
91  ((prev) && (!(prev)->isStaticOrKinematicObject())))
92  {
93  int tagPrev = prev->getIslandTag();
94  int tagCur = cur->getIslandTag();
95  getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
96  }
97  if (cur && !cur->isStaticOrKinematicObject())
98  prev = cur;
99  }
100  }
101  }
102 
103  //merge islands linked by multibody constraints
104  {
105  for (int i = 0; i < this->m_multiBodyConstraints.size(); i++)
106  {
108  int tagA = c->getIslandIdA();
109  int tagB = c->getIslandIdB();
110  if (tagA >= 0 && tagB >= 0)
112  }
113  }
114 
115  //Store the island id in each body
117 }
118 
120 {
121  BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
122 
123  for (int i = 0; i < m_multiBodies.size(); i++)
124  {
125  btMultiBody* body = m_multiBodies[i];
126  if (body)
127  {
128  body->checkMotionAndSleepIfRequired(timeStep);
129  if (!body->isAwake())
130  {
132  if (col && col->getActivationState() == ACTIVE_TAG)
133  {
135  col->setDeactivationTime(0.f);
136  }
137  for (int b = 0; b < body->getNumLinks(); b++)
138  {
140  if (col && col->getActivationState() == ACTIVE_TAG)
141  {
143  col->setDeactivationTime(0.f);
144  }
145  }
146  }
147  else
148  {
150  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
152 
153  for (int b = 0; b < body->getNumLinks(); b++)
154  {
156  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
158  }
159  }
160  }
161  }
162 
164 }
165 
167 {
168  int islandId;
169 
170  const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
171  const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
172  islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
173  return islandId;
174 }
175 
177 {
178 public:
179  bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
180  {
181  int rIslandId0, lIslandId0;
182  rIslandId0 = btGetConstraintIslandId2(rhs);
183  lIslandId0 = btGetConstraintIslandId2(lhs);
184  return lIslandId0 < rIslandId0;
185  }
186 };
187 
189 {
190  int islandId;
191 
192  int islandTagA = lhs->getIslandIdA();
193  int islandTagB = lhs->getIslandIdB();
194  islandId = islandTagA >= 0 ? islandTagA : islandTagB;
195  return islandId;
196 }
197 
199 {
200 public:
201  bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
202  {
203  int rIslandId0, lIslandId0;
204  rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
205  lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
206  return lIslandId0 < rIslandId0;
207  }
208 };
209 
211 {
216 
221 
226 
228  btDispatcher* dispatcher)
229  : m_solverInfo(NULL),
230  m_solver(solver),
232  m_numConstraints(0),
233  m_debugDrawer(NULL),
234  m_dispatcher(dispatcher)
235  {
236  }
237 
239  {
240  btAssert(0);
241  (void)other;
242  return *this;
243  }
244 
245  SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
246  {
247  btAssert(solverInfo);
248  m_solverInfo = solverInfo;
249 
250  m_multiBodySortedConstraints = sortedMultiBodyConstraints;
251  m_numMultiBodyConstraints = numMultiBodyConstraints;
252  m_sortedConstraints = sortedConstraints;
253  m_numConstraints = numConstraints;
254 
255  m_debugDrawer = debugDrawer;
256  m_bodies.resize(0);
257  m_manifolds.resize(0);
260  }
261 
263  {
264  m_solver = solver;
265  }
266 
267  virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
268  {
269  if (islandId < 0)
270  {
273  }
274  else
275  {
276  //also add all non-contact constraints/joints for this island
277  btTypedConstraint** startConstraint = 0;
278  btMultiBodyConstraint** startMultiBodyConstraint = 0;
279 
280  int numCurConstraints = 0;
281  int numCurMultiBodyConstraints = 0;
282 
283  int i;
284 
285  //find the first constraint for this island
286 
287  for (i = 0; i < m_numConstraints; i++)
288  {
289  if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
290  {
291  startConstraint = &m_sortedConstraints[i];
292  break;
293  }
294  }
295  //count the number of constraints in this island
296  for (; i < m_numConstraints; i++)
297  {
298  if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
299  {
300  numCurConstraints++;
301  }
302  }
303 
304  for (i = 0; i < m_numMultiBodyConstraints; i++)
305  {
307  {
308  startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
309  break;
310  }
311  }
312  //count the number of multi body constraints in this island
313  for (; i < m_numMultiBodyConstraints; i++)
314  {
316  {
317  numCurMultiBodyConstraints++;
318  }
319  }
320 
321  //if (m_solverInfo->m_minimumSolverBatchSize<=1)
322  //{
323  // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
324  //} else
325  {
326  for (i = 0; i < numBodies; i++)
327  m_bodies.push_back(bodies[i]);
328  for (i = 0; i < numManifolds; i++)
329  m_manifolds.push_back(manifolds[i]);
330  for (i = 0; i < numCurConstraints; i++)
331  m_constraints.push_back(startConstraint[i]);
332 
333  for (i = 0; i < numCurMultiBodyConstraints; i++)
334  m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
335 
337  {
339  }
340  else
341  {
342  //printf("deferred\n");
343  }
344  }
345  }
346  }
348  {
349  btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
350  btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
351  btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
352  btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
353 
354  //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
355 
356  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);
357  m_bodies.resize(0);
358  m_manifolds.resize(0);
361  }
362 };
363 
365  : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
366  m_multiBodyConstraintSolver(constraintSolver)
367 {
368  //split impulse is not yet supported for Featherstone hierarchies
369  // getSolverInfo().m_splitImpulse = false;
371  m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
372 }
373 
375 {
377 }
378 
380 {
384 }
385 
387 {
388  if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
389  {
391  }
393 }
394 
396 {
397  for (int b = 0; b < m_multiBodies.size(); b++)
398  {
399  btMultiBody* bod = m_multiBodies[b];
401  }
402 }
404 {
406 
407  BT_PROFILE("solveConstraints");
408 
410 
412  int i;
413  for (i = 0; i < getNumConstraints(); i++)
414  {
416  }
418  btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
419 
421  for (i = 0; i < m_multiBodyConstraints.size(); i++)
422  {
424  }
426 
427  btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
428 
429  m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
431 
432 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
433  {
434  BT_PROFILE("btMultiBody addForce");
435  for (int i = 0; i < this->m_multiBodies.size(); i++)
436  {
437  btMultiBody* bod = m_multiBodies[i];
438 
439  bool isSleeping = false;
440 
442  {
443  isSleeping = true;
444  }
445  for (int b = 0; b < bod->getNumLinks(); b++)
446  {
448  isSleeping = true;
449  }
450 
451  if (!isSleeping)
452  {
453  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
454  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
455  m_scratch_v.resize(bod->getNumLinks() + 1);
456  m_scratch_m.resize(bod->getNumLinks() + 1);
457 
458  bod->addBaseForce(m_gravity * bod->getBaseMass());
459 
460  for (int j = 0; j < bod->getNumLinks(); ++j)
461  {
462  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
463  }
464  } //if (!isSleeping)
465  }
466  }
467 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
468 
469  {
470  BT_PROFILE("btMultiBody stepVelocities");
471  for (int i = 0; i < this->m_multiBodies.size(); i++)
472  {
473  btMultiBody* bod = m_multiBodies[i];
474 
475  bool isSleeping = false;
476 
478  {
479  isSleeping = true;
480  }
481  for (int b = 0; b < bod->getNumLinks(); b++)
482  {
484  isSleeping = true;
485  }
486 
487  if (!isSleeping)
488  {
489  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
490  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
491  m_scratch_v.resize(bod->getNumLinks() + 1);
492  m_scratch_m.resize(bod->getNumLinks() + 1);
493  bool doNotUpdatePos = false;
494  bool isConstraintPass = false;
495  {
496  if (!bod->isUsingRK4Integration())
497  {
499  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
500  getSolverInfo().m_jointFeedbackInWorldSpace,
501  getSolverInfo().m_jointFeedbackInJointFrame);
502  }
503  else
504  {
505  //
506  int numDofs = bod->getNumDofs() + 6;
507  int numPosVars = bod->getNumPosVars() + 7;
509  scratch_r2.resize(2 * numPosVars + 8 * numDofs);
510  //convenience
511  btScalar* pMem = &scratch_r2[0];
512  btScalar* scratch_q0 = pMem;
513  pMem += numPosVars;
514  btScalar* scratch_qx = pMem;
515  pMem += numPosVars;
516  btScalar* scratch_qd0 = pMem;
517  pMem += numDofs;
518  btScalar* scratch_qd1 = pMem;
519  pMem += numDofs;
520  btScalar* scratch_qd2 = pMem;
521  pMem += numDofs;
522  btScalar* scratch_qd3 = pMem;
523  pMem += numDofs;
524  btScalar* scratch_qdd0 = pMem;
525  pMem += numDofs;
526  btScalar* scratch_qdd1 = pMem;
527  pMem += numDofs;
528  btScalar* scratch_qdd2 = pMem;
529  pMem += numDofs;
530  btScalar* scratch_qdd3 = pMem;
531  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  } 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 &m_scratch_r[bod->getNumDofs()]
598  //calc qdd0 from: q0 & qd0
600  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
601  getSolverInfo().m_jointFeedbackInJointFrame);
602  pCopy(output, scratch_qdd0, 0, numDofs);
603  //calc q1 = q0 + h/2 * qd0
604  pResetQx();
605  bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
606  //calc qd1 = qd0 + h/2 * qdd0
607  pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
608  //
609  //calc qdd1 from: q1 & qd1
610  pCopyToVelocityVector(bod, scratch_qd1);
612  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
613  getSolverInfo().m_jointFeedbackInJointFrame);
614  pCopy(output, scratch_qdd1, 0, numDofs);
615  //calc q2 = q0 + h/2 * qd1
616  pResetQx();
617  bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
618  //calc qd2 = qd0 + h/2 * qdd1
619  pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
620  //
621  //calc qdd2 from: q2 & qd2
622  pCopyToVelocityVector(bod, scratch_qd2);
624  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
625  getSolverInfo().m_jointFeedbackInJointFrame);
626  pCopy(output, scratch_qdd2, 0, numDofs);
627  //calc q3 = q0 + h * qd2
628  pResetQx();
629  bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
630  //calc qd3 = qd0 + h * qdd2
631  pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
632  //
633  //calc qdd3 from: q3 & qd3
634  pCopyToVelocityVector(bod, scratch_qd3);
636  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
637  getSolverInfo().m_jointFeedbackInJointFrame);
638  pCopy(output, scratch_qdd3, 0, numDofs);
639 
640  //
641  //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
642  //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
644  delta_q.resize(numDofs);
646  delta_qd.resize(numDofs);
647  for (int i = 0; i < numDofs; ++i)
648  {
649  delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
650  delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
651  //delta_q[i] = h*scratch_qd0[i];
652  //delta_qd[i] = h*scratch_qdd0[i];
653  }
654  //
655  pCopyToVelocityVector(bod, scratch_qd0);
656  bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
657  //
658  if (!doNotUpdatePos)
659  {
660  btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
661  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
662 
663  for (int i = 0; i < numDofs; ++i)
664  pRealBuf[i] = delta_q[i];
665 
666  //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
667  bod->setPosUpdated(true);
668  }
669 
670  //ugly hack which resets the cached data to t0 (needed for constraint solver)
671  {
672  for (int link = 0; link < bod->getNumLinks(); ++link)
673  bod->getLink(link).updateCacheMultiDof();
675  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
676  getSolverInfo().m_jointFeedbackInJointFrame);
677  }
678  }
679  }
680 
681 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
682  bod->clearForcesAndTorques();
683 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
684  } //if (!isSleeping)
685  }
686  }
687 
690 
692 
694 
695  {
696  BT_PROFILE("btMultiBody stepVelocities");
697  for (int i = 0; i < this->m_multiBodies.size(); i++)
698  {
699  btMultiBody* bod = m_multiBodies[i];
700 
701  bool isSleeping = false;
702 
704  {
705  isSleeping = true;
706  }
707  for (int b = 0; b < bod->getNumLinks(); b++)
708  {
710  isSleeping = true;
711  }
712 
713  if (!isSleeping)
714  {
715  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
716  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
717  m_scratch_v.resize(bod->getNumLinks() + 1);
718  m_scratch_m.resize(bod->getNumLinks() + 1);
719 
720  {
721  if (!bod->isUsingRK4Integration())
722  {
723  bool isConstraintPass = true;
725  getSolverInfo().m_jointFeedbackInWorldSpace,
726  getSolverInfo().m_jointFeedbackInJointFrame);
727  }
728  }
729  }
730  }
731  }
732 
733  for (int i = 0; i < this->m_multiBodies.size(); i++)
734  {
735  btMultiBody* bod = m_multiBodies[i];
737  }
738 }
739 
741 {
743 
744  {
745  BT_PROFILE("btMultiBody stepPositions");
746  //integrate and update the Featherstone hierarchies
747 
748  for (int b = 0; b < m_multiBodies.size(); b++)
749  {
750  btMultiBody* bod = m_multiBodies[b];
751  bool isSleeping = false;
753  {
754  isSleeping = true;
755  }
756  for (int b = 0; b < bod->getNumLinks(); b++)
757  {
759  isSleeping = true;
760  }
761 
762  if (!isSleeping)
763  {
764  int nLinks = bod->getNumLinks();
765 
767 
768  {
769  if (!bod->isPosUpdated())
770  bod->stepPositionsMultiDof(timeStep);
771  else
772  {
773  btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
774  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
775 
776  bod->stepPositionsMultiDof(1, 0, pRealBuf);
777  bod->setPosUpdated(false);
778  }
779  }
780 
781  m_scratch_world_to_local.resize(nLinks + 1);
782  m_scratch_local_origin.resize(nLinks + 1);
783 
785  }
786  else
787  {
788  bod->clearVelocities();
789  }
790  }
791  }
792 }
793 
795 {
796  m_multiBodyConstraints.push_back(constraint);
797 }
798 
800 {
801  m_multiBodyConstraints.remove(constraint);
802 }
803 
805 {
806  constraint->debugDraw(getDebugDrawer());
807 }
808 
810 {
811  BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
812 
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  for (int c = 0; c < m_multiBodyConstraints.size(); c++)
829  {
831  debugDrawMultiBodyConstraint(constraint);
832  }
833 
834  for (int b = 0; b < m_multiBodies.size(); b++)
835  {
836  btMultiBody* bod = m_multiBodies[b];
838 
839  if (mode & btIDebugDraw::DBG_DrawFrames)
840  {
842  }
843 
844  for (int m = 0; m < bod->getNumLinks(); m++)
845  {
846  const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
847  if (mode & btIDebugDraw::DBG_DrawFrames)
848  {
849  getDebugDrawer()->drawTransform(tr, 0.1);
850  }
851  //draw the joint axis
853  {
854  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
855 
856  btVector4 color(0, 0, 0, 1); //1,1,1);
857  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
858  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
859  getDebugDrawer()->drawLine(from, to, color);
860  }
862  {
863  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
864 
865  btVector4 color(0, 0, 0, 1); //1,1,1);
866  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
867  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
868  getDebugDrawer()->drawLine(from, to, color);
869  }
871  {
872  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
873 
874  btVector4 color(0, 0, 0, 1); //1,1,1);
875  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
876  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
877  getDebugDrawer()->drawLine(from, to, color);
878  }
879  }
880  }
881  }
882  }
883 }
884 
886 {
888 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
889  BT_PROFILE("btMultiBody addGravity");
890  for (int i = 0; i < this->m_multiBodies.size(); i++)
891  {
892  btMultiBody* bod = m_multiBodies[i];
893 
894  bool isSleeping = false;
895 
897  {
898  isSleeping = true;
899  }
900  for (int b = 0; b < bod->getNumLinks(); b++)
901  {
903  isSleeping = true;
904  }
905 
906  if (!isSleeping)
907  {
908  bod->addBaseForce(m_gravity * bod->getBaseMass());
909 
910  for (int j = 0; j < bod->getNumLinks(); ++j)
911  {
912  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
913  }
914  } //if (!isSleeping)
915  }
916 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
917 }
918 
920 {
921  for (int i = 0; i < this->m_multiBodies.size(); i++)
922  {
923  btMultiBody* bod = m_multiBodies[i];
924  bod->clearConstraintForces();
925  }
926 }
928 {
929  {
930  // BT_PROFILE("clearMultiBodyForces");
931  for (int i = 0; i < this->m_multiBodies.size(); i++)
932  {
933  btMultiBody* bod = m_multiBodies[i];
934 
935  bool isSleeping = false;
936 
938  {
939  isSleeping = true;
940  }
941  for (int b = 0; b < bod->getNumLinks(); b++)
942  {
944  isSleeping = true;
945  }
946 
947  if (!isSleeping)
948  {
949  btMultiBody* bod = m_multiBodies[i];
950  bod->clearForcesAndTorques();
951  }
952  }
953  }
954 }
956 {
958 
959 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
961 #endif
962 }
963 
965 {
966  serializer->startSerialization();
967 
968  serializeDynamicsWorldInfo(serializer);
969 
970  serializeMultiBodies(serializer);
971 
972  serializeRigidBodies(serializer);
973 
974  serializeCollisionObjects(serializer);
975 
976  serializeContactManifolds(serializer);
977 
978  serializer->finishSerialization();
979 }
980 
982 {
983  int i;
984  //serialize all collision objects
985  for (i = 0; i < m_multiBodies.size(); i++)
986  {
987  btMultiBody* mb = m_multiBodies[i];
988  {
989  int len = mb->calculateSerializeBufferSize();
990  btChunk* chunk = serializer->allocate(len, 1);
991  const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
992  serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
993  }
994  }
995 
996  //serialize all multibody links (collision objects)
997  for (i = 0; i < m_collisionObjects.size(); i++)
998  {
1001  {
1002  int len = colObj->calculateSerializeBufferSize();
1003  btChunk* chunk = serializer->allocate(len, 1);
1004  const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
1005  serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
1006  }
1007  }
1008 }
btCollisionWorld::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btCollisionWorld.h:96
MultiBodyInplaceSolverIslandCallback::m_numConstraints
int m_numConstraints
Definition: btMultiBodyDynamicsWorld.cpp:218
btDiscreteDynamicsWorld::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btDiscreteDynamicsWorld.h:47
btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
Definition: btMultiBodyDynamicsWorld.cpp:379
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:74
btSimulationIslandManager.h
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btDiscreteDynamicsWorld::setConstraintSolver
virtual void setConstraintSolver(btConstraintSolver *solver)
Definition: btDiscreteDynamicsWorld.cpp:1342
btUnionFind::unite
void unite(int p, int q)
Definition: btUnionFind.h:76
btSimulationIslandManager::storeIslandActivationState
virtual void storeIslandActivationState(btCollisionWorld *world)
Definition: btSimulationIslandManager.cpp:141
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
MultiBodyInplaceSolverIslandCallback::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.cpp:225
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
DISABLE_DEACTIVATION
#define DISABLE_DEACTIVATION
Definition: btCollisionObject.h:25
btMultiBodyDynamicsWorld::calculateSimulationIslands
virtual void calculateSimulationIslands()
Definition: btMultiBodyDynamicsWorld.cpp:36
btMultiBodyDynamicsWorld::addMultiBodyConstraint
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:794
btMultiBodyDynamicsWorld::m_scratch_world_to_local
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local
Definition: btMultiBodyDynamicsWorld.h:40
btCollisionWorld::getDebugDrawer
virtual btIDebugDraw * getDebugDrawer()
Definition: btCollisionWorld.h:155
btMultiBody::processDeltaVeeMultiDof2
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:384
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btSimulationIslandManager::buildAndProcessIslands
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
Definition: btSimulationIslandManager.cpp:341
btDiscreteDynamicsWorld::m_sortedConstraints
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
Definition: btDiscreteDynamicsWorld.h:40
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:166
btMultiBodyDynamicsWorld.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMultiBodyDynamicsWorld::m_scratch_local_origin1
btAlignedObjectArray< btVector3 > m_scratch_local_origin1
Definition: btMultiBodyDynamicsWorld.h:43
MultiBodyInplaceSolverIslandCallback::m_multiBodySortedConstraints
btMultiBodyConstraint ** m_multiBodySortedConstraints
Definition: btMultiBodyDynamicsWorld.cpp:214
btCollisionObject::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btCollisionObject.cpp:80
ACTIVE_TAG
#define ACTIVE_TAG
Definition: btCollisionObject.h:22
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
btDiscreteDynamicsWorld::m_islandManager
btSimulationIslandManager * m_islandManager
Definition: btDiscreteDynamicsWorld.h:45
btSortConstraintOnIslandPredicate2::operator()
bool operator()(const btTypedConstraint *lhs, const btTypedConstraint *rhs) const
Definition: btMultiBodyDynamicsWorld.cpp:179
btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces
virtual void clearMultiBodyConstraintForces()
Definition: btMultiBodyDynamicsWorld.cpp:919
btSortMultiBodyConstraintOnIslandPredicate::operator()
bool operator()(const btMultiBodyConstraint *lhs, const btMultiBodyConstraint *rhs) const
Definition: btMultiBodyDynamicsWorld.cpp:201
MultiBodyInplaceSolverIslandCallback::setMultiBodyConstraintSolver
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
Definition: btMultiBodyDynamicsWorld.cpp:262
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:348
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:190
btMultiBodyConstraintSolver.h
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
btChunk
Definition: btSerializer.h:47
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:619
btMultiBodyConstraint::getIslandIdB
virtual int getIslandIdB() const =0
btMultiBody::isPosUpdated
bool isPosUpdated() const
Definition: btMultiBody.h:545
btCollisionWorld::m_collisionObjects
btAlignedObjectArray< btCollisionObject * > m_collisionObjects
Definition: btCollisionWorld.h:88
btIDebugDraw::getDebugMode
virtual int getDebugMode() const =0
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
btMultiBodyConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)=0
btDynamicsWorld::getSolverInfo
btContactSolverInfo & getSolverInfo()
Definition: btDynamicsWorld.h:139
btMultiBodyDynamicsWorld::m_solverMultiBodyIslandCallback
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
Definition: btMultiBodyDynamicsWorld.h:37
btCollisionObject::getActivationState
int getActivationState() const
Definition: btCollisionObject.h:275
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1951
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:2002
MultiBodyInplaceSolverIslandCallback::MultiBodyInplaceSolverIslandCallback
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)
Definition: btMultiBodyDynamicsWorld.cpp:227
btCollisionWorld::serializeContactManifolds
void serializeContactManifolds(btSerializer *serializer)
Definition: btCollisionWorld.cpp:1583
BT_MULTIBODY_CODE
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:108
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
btDiscreteDynamicsWorld::getSimulationIslandManager
btSimulationIslandManager * getSimulationIslandManager()
Definition: btDiscreteDynamicsWorld.h:123
BT_MB_LINKCOLLIDER_CODE
#define BT_MB_LINKCOLLIDER_CODE
Definition: btSerializer.h:109
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:207
btMultiBody::isUsingRK4Integration
bool isUsingRK4Integration() const
Definition: btMultiBody.h:541
btSortConstraintOnIslandPredicate2
Definition: btMultiBodyDynamicsWorld.cpp:176
btMultiBody::stepPositionsMultiDof
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
Definition: btMultiBody.cpp:1583
btMultiBodyDynamicsWorld::solveConstraints
virtual void solveConstraints(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:403
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btVector4
Definition: btVector3.h:1073
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:203
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:394
btMultiBodyDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
Definition: btMultiBodyDynamicsWorld.cpp:955
btMultiBodyDynamicsWorld::m_scratch_r
btAlignedObjectArray< btScalar > m_scratch_r
Definition: btMultiBodyDynamicsWorld.h:44
btConstraintSolver::getSolverType
virtual btConstraintSolverType getSolverType() const =0
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btAssert
#define btAssert(x)
Definition: btScalar.h:133
MultiBodyInplaceSolverIslandCallback::m_manifolds
btAlignedObjectArray< btPersistentManifold * > m_manifolds
Definition: btMultiBodyDynamicsWorld.cpp:223
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:630
btMultiBody::addLinkForce
void addLinkForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:650
btDiscreteDynamicsWorld::getCollisionWorld
btCollisionWorld * getCollisionWorld()
Definition: btDiscreteDynamicsWorld.h:133
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:201
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:26
MultiBodyInplaceSolverIslandCallback
Definition: btMultiBodyDynamicsWorld.cpp:210
btMultiBodyDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btMultiBodyDynamicsWorld.cpp:809
btIDebugDraw::DBG_DrawConstraints
Definition: btIDebugDraw.h:66
MultiBodyInplaceSolverIslandCallback::setup
void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
Definition: btMultiBodyDynamicsWorld.cpp:245
btGetConstraintIslandId2
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
Definition: btMultiBodyDynamicsWorld.cpp:166
btMultiBodyDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:740
btMultiBody::checkMotionAndSleepIfRequired
void checkMotionAndSleepIfRequired(btScalar timestep)
Definition: btMultiBody.cpp:1875
btMultiBodyDynamicsWorld::m_multiBodies
btAlignedObjectArray< btMultiBody * > m_multiBodies
Definition: btMultiBodyDynamicsWorld.h:33
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:41
btSortMultiBodyConstraintOnIslandPredicate
Definition: btMultiBodyDynamicsWorld.cpp:198
btMultiBodyConstraintSolver
Definition: btMultiBodyConstraintSolver.h:28
btIDebugDraw::DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:67
btDiscreteDynamicsWorld::m_gravity
btVector3 m_gravity
Definition: btDiscreteDynamicsWorld.h:51
btDiscreteDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btDiscreteDynamicsWorld.cpp:269
MultiBodyInplaceSolverIslandCallback::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btMultiBodyDynamicsWorld.cpp:219
btMultiBody::addBaseForce
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:304
btMultiBody::isAwake
bool isAwake() const
Definition: btMultiBody.h:462
MultiBodyInplaceSolverIslandCallback::m_bodies
btAlignedObjectArray< btCollisionObject * > m_bodies
Definition: btMultiBodyDynamicsWorld.cpp:222
btMultiBodyDynamicsWorld::m_scratch_local_origin
btAlignedObjectArray< btVector3 > m_scratch_local_origin
Definition: btMultiBodyDynamicsWorld.h:41
btMultiBody::getBaseMass
btScalar getBaseMass() const
Definition: btMultiBody.h:169
btConstraintSolver::allSolved
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
Definition: btConstraintSolver.h:50
btCollisionWorld::getNumCollisionObjects
int getNumCollisionObjects() const
Definition: btCollisionWorld.h:427
btSerializer.h
btCollisionObject::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btCollisionObject.h:661
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:279
btMultiBodyConstraint::getIslandIdA
virtual int getIslandIdA() const =0
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1908
btMultiBodyDynamicsWorld::m_multiBodyConstraintSolver
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
Definition: btMultiBodyDynamicsWorld.h:36
btIDebugDraw::DBG_DrawFrames
Definition: btIDebugDraw.h:70
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btSimulationIslandManager::IslandCallback
Definition: btSimulationIslandManager.h:52
btMultiBodyDynamicsWorld::m_sortedMultiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:35
btMultiBodyDynamicsWorld::removeMultiBodyConstraint
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:799
btDiscreteDynamicsWorld::m_predictiveManifolds
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
Definition: btDiscreteDynamicsWorld.h:69
btMultiBody::getNumPosVars
int getNumPosVars() const
Definition: btMultiBody.h:168
btSimulationIslandManager::updateActivationState
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
Definition: btSimulationIslandManager.cpp:119
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:58
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btMultiBody
Definition: btMultiBody.h:49
MultiBodyInplaceSolverIslandCallback::m_dispatcher
btDispatcher * m_dispatcher
Definition: btMultiBodyDynamicsWorld.cpp:220
btDiscreteDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btDiscreteDynamicsWorld.cpp:322
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btMultiBodyDynamicsWorld::forwardKinematics
void forwardKinematics()
Definition: btMultiBodyDynamicsWorld.cpp:395
btMultiBodyDynamicsWorld::m_scratch_v
btAlignedObjectArray< btVector3 > m_scratch_v
Definition: btMultiBodyDynamicsWorld.h:45
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:434
btSerializer::finishSerialization
virtual void finishSerialization()=0
btSerializer::startSerialization
virtual void startSerialization()=0
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
output
#define output
btCollisionObject::CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:151
btMultiBodyDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btMultiBodyDynamicsWorld.cpp:885
btAlignedObjectArray< btCollisionObject * >
MultiBodyInplaceSolverIslandCallback::operator=
MultiBodyInplaceSolverIslandCallback & operator=(MultiBodyInplaceSolverIslandCallback &other)
Definition: btMultiBodyDynamicsWorld.cpp:238
MultiBodyInplaceSolverIslandCallback::m_sortedConstraints
btTypedConstraint ** m_sortedConstraints
Definition: btMultiBodyDynamicsWorld.cpp:217
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:358
MultiBodyInplaceSolverIslandCallback::processIsland
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
Definition: btMultiBodyDynamicsWorld.cpp:267
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:83
btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
Definition: btMultiBody.cpp:730
btMultiBody::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btMultiBody.cpp:2009
btDiscreteDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:1039
btSerializer
Definition: btSerializer.h:65
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:549
btMultiBody.h
btMultiBodyDynamicsWorld::m_scratch_m
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
Definition: btMultiBodyDynamicsWorld.h:46
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld
virtual ~btMultiBodyDynamicsWorld()
Definition: btMultiBodyDynamicsWorld.cpp:374
MultiBodyInplaceSolverIslandCallback::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btMultiBodyDynamicsWorld.cpp:224
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
Definition: btMultiBodyDynamicsWorld.cpp:364
btMultiBody::clearVelocities
void clearVelocities()
Definition: btMultiBody.cpp:643
btBroadphaseInterface
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
Definition: btBroadphaseInterface.h:49
btIDebugDraw.h
btMultiBodyDynamicsWorld::setConstraintSolver
virtual void setConstraintSolver(btConstraintSolver *solver)
Definition: btMultiBodyDynamicsWorld.cpp:386
btQuickprof.h
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:256
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:360
btDiscreteDynamicsWorld::m_constraintSolver
btConstraintSolver * m_constraintSolver
Definition: btDiscreteDynamicsWorld.h:43
btMultiBodyDynamicsWorld::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:34
btDiscreteDynamicsWorld::getNumConstraints
virtual int getNumConstraints() const
Definition: btDiscreteDynamicsWorld.cpp:1358
WANTS_DEACTIVATION
#define WANTS_DEACTIVATION
Definition: btCollisionObject.h:24
btDiscreteDynamicsWorld::serializeRigidBodies
void serializeRigidBodies(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1371
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btMultiBodyDynamicsWorld::clearMultiBodyForces
virtual void clearMultiBodyForces()
Definition: btMultiBodyDynamicsWorld.cpp:927
btCollisionWorld::getDispatcher
btDispatcher * getDispatcher()
Definition: btCollisionWorld.h:132
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btCollisionWorld::serializeCollisionObjects
void serializeCollisionObjects(btSerializer *serializer)
Definition: btCollisionWorld.cpp:1553
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btMultiBodyConstraint.h
btSimulationIslandManager::getUnionFind
btUnionFind & getUnionFind()
Definition: btSimulationIslandManager.h:45
btDiscreteDynamicsWorld
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
Definition: btDiscreteDynamicsWorld.h:36
btMultiBodyDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:119
btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:804
btMultiBodyDynamicsWorld::serialize
virtual void serialize(btSerializer *serializer)
Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see B...
Definition: btMultiBodyDynamicsWorld.cpp:964
btContactSolverInfoData::m_minimumSolverBatchSize
int m_minimumSolverBatchSize
Definition: btContactSolverInfo.h:60
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:264
MultiBodyInplaceSolverIslandCallback::m_solver
btMultiBodyConstraintSolver * m_solver
Definition: btMultiBodyDynamicsWorld.cpp:213
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btConstraintSolver::prepareSolve
virtual void prepareSolve(int, int)
Definition: btConstraintSolver.h:45
btMultiBodyDynamicsWorld::serializeMultiBodies
virtual void serializeMultiBodies(btSerializer *serializer)
Definition: btMultiBodyDynamicsWorld.cpp:981
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btCollisionConfiguration
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size,...
Definition: btCollisionConfiguration.h:26
btDiscreteDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:594
btAlignedObjectArray::remove
void remove(const T &key)
Definition: btAlignedObjectArray.h:487
btGetMultiBodyConstraintIslandId
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
Definition: btMultiBodyDynamicsWorld.cpp:188
BT_MULTIBODY_SOLVER
Definition: btConstraintSolver.h:37
MultiBodyInplaceSolverIslandCallback::m_numMultiBodyConstraints
int m_numMultiBodyConstraints
Definition: btMultiBodyDynamicsWorld.cpp:215
btMultiBodyDynamicsWorld::removeMultiBody
virtual void removeMultiBody(btMultiBody *body)
Definition: btMultiBodyDynamicsWorld.cpp:31
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:197
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMultiBodyDynamicsWorld::m_scratch_world_to_local1
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local1
Definition: btMultiBodyDynamicsWorld.h:42
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btMultiBodyConstraintSolver::solveMultiBodyGroup
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
Definition: btMultiBodyConstraintSolver.cpp:1615
btDiscreteDynamicsWorld::serializeDynamicsWorldInfo
void serializeDynamicsWorldInfo(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1397
MultiBodyInplaceSolverIslandCallback::m_solverInfo
btContactSolverInfo * m_solverInfo
Definition: btMultiBodyDynamicsWorld.cpp:212
MultiBodyInplaceSolverIslandCallback::processConstraints
void processConstraints()
Definition: btMultiBodyDynamicsWorld.cpp:347
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btConstraintSolver
Definition: btConstraintSolver.h:40
btDiscreteDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
Definition: btDiscreteDynamicsWorld.cpp:309
btCollisionObject::setActivationState
void setActivationState(int newState) const
Definition: btCollisionObject.cpp:60
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btMultiBodyDynamicsWorld::addMultiBody
virtual void addMultiBody(btMultiBody *body, int group=btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter)
Definition: btMultiBodyDynamicsWorld.cpp:26
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926