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 {
40 
41 }
43 {
44  BT_PROFILE("calculateSimulationIslands");
45 
47 
48  {
49  //merge islands based on speculative contact manifolds too
50  for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
51  {
53 
54  const btCollisionObject* colObj0 = manifold->getBody0();
55  const btCollisionObject* colObj1 = manifold->getBody1();
56 
57  if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
58  ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
59  {
60  getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
61  }
62  }
63  }
64 
65  {
66  int i;
67  int numConstraints = int(m_constraints.size());
68  for (i = 0; i < numConstraints; i++)
69  {
70  btTypedConstraint* constraint = m_constraints[i];
71  if (constraint->isEnabled())
72  {
73  const btRigidBody* colObj0 = &constraint->getRigidBodyA();
74  const btRigidBody* colObj1 = &constraint->getRigidBodyB();
75 
76  if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
77  ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
78  {
79  getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
80  }
81  }
82  }
83  }
84 
85  //merge islands linked by Featherstone link colliders
86  for (int i = 0; i < m_multiBodies.size(); i++)
87  {
88  btMultiBody* body = m_multiBodies[i];
89  {
91 
92  for (int b = 0; b < body->getNumLinks(); b++)
93  {
95 
96  if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
97  ((prev) && (!(prev)->isStaticOrKinematicObject())))
98  {
99  int tagPrev = prev->getIslandTag();
100  int tagCur = cur->getIslandTag();
101  getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
102  }
103  if (cur && !cur->isStaticOrKinematicObject())
104  prev = cur;
105  }
106  }
107  }
108 
109  //merge islands linked by multibody constraints
110  {
111  for (int i = 0; i < this->m_multiBodyConstraints.size(); i++)
112  {
114  int tagA = c->getIslandIdA();
115  int tagB = c->getIslandIdB();
116  if (tagA >= 0 && tagB >= 0)
118  }
119  }
120 
121  //Store the island id in each body
123 }
124 
126 {
127  BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
128 
129  for (int i = 0; i < m_multiBodies.size(); i++)
130  {
131  btMultiBody* body = m_multiBodies[i];
132  if (body)
133  {
134  body->checkMotionAndSleepIfRequired(timeStep);
135  if (!body->isAwake())
136  {
138  if (col && col->getActivationState() == ACTIVE_TAG)
139  {
141  col->setDeactivationTime(0.f);
142  }
143  for (int b = 0; b < body->getNumLinks(); b++)
144  {
146  if (col && col->getActivationState() == ACTIVE_TAG)
147  {
149  col->setDeactivationTime(0.f);
150  }
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 
170 }
171 
173 {
175 }
176 
178  : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
179  m_multiBodyConstraintSolver(constraintSolver)
180 {
181  //split impulse is not yet supported for Featherstone hierarchies
182  // getSolverInfo().m_splitImpulse = false;
184  m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
185 }
186 
188 {
190 }
191 
193 {
197 }
198 
200 {
201  if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
202  {
204  }
206 }
207 
209 {
210  for (int b = 0; b < m_multiBodies.size(); b++)
211  {
212  btMultiBody* bod = m_multiBodies[b];
214  }
215 }
217 {
218  solveExternalForces(solverInfo);
219  buildIslands();
220  solveInternalConstraints(solverInfo);
221 }
222 
224 {
226 }
227 
229 {
233  {
234  BT_PROFILE("btMultiBody stepVelocities");
235  for (int i = 0; i < this->m_multiBodies.size(); i++)
236  {
237  btMultiBody* bod = m_multiBodies[i];
238 
239  bool isSleeping = false;
240 
242  {
243  isSleeping = true;
244  }
245  for (int b = 0; b < bod->getNumLinks(); b++)
246  {
248  isSleeping = true;
249  }
250 
251  if (!isSleeping)
252  {
253  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
254  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
255  m_scratch_v.resize(bod->getNumLinks() + 1);
256  m_scratch_m.resize(bod->getNumLinks() + 1);
257 
258  if (bod->internalNeedsJointFeedback())
259  {
260  if (!bod->isUsingRK4Integration())
261  {
262  if (bod->internalNeedsJointFeedback())
263  {
264  bool isConstraintPass = true;
266  getSolverInfo().m_jointFeedbackInWorldSpace,
267  getSolverInfo().m_jointFeedbackInJointFrame);
268  }
269  }
270  }
271  }
272  }
273  }
274  for (int i = 0; i < this->m_multiBodies.size(); i++)
275  {
276  btMultiBody* bod = m_multiBodies[i];
278  }
279 }
280 
282 {
284 
285  BT_PROFILE("solveConstraints");
286 
288 
290  int i;
291  for (i = 0; i < getNumConstraints(); i++)
292  {
294  }
296  btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
297 
299  for (i = 0; i < m_multiBodyConstraints.size(); i++)
300  {
302  }
304 
305  btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
306 
307  m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
309 
310 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
311  {
312  BT_PROFILE("btMultiBody addForce");
313  for (int i = 0; i < this->m_multiBodies.size(); i++)
314  {
315  btMultiBody* bod = m_multiBodies[i];
316 
317  bool isSleeping = false;
318 
320  {
321  isSleeping = true;
322  }
323  for (int b = 0; b < bod->getNumLinks(); b++)
324  {
326  isSleeping = true;
327  }
328 
329  if (!isSleeping)
330  {
331  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
332  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
333  m_scratch_v.resize(bod->getNumLinks() + 1);
334  m_scratch_m.resize(bod->getNumLinks() + 1);
335 
336  bod->addBaseForce(m_gravity * bod->getBaseMass());
337 
338  for (int j = 0; j < bod->getNumLinks(); ++j)
339  {
340  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
341  }
342  } //if (!isSleeping)
343  }
344  }
345 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
346 
347  {
348  BT_PROFILE("btMultiBody stepVelocities");
349  for (int i = 0; i < this->m_multiBodies.size(); i++)
350  {
351  btMultiBody* bod = m_multiBodies[i];
352 
353  bool isSleeping = false;
354 
356  {
357  isSleeping = true;
358  }
359  for (int b = 0; b < bod->getNumLinks(); b++)
360  {
362  isSleeping = true;
363  }
364 
365  if (!isSleeping)
366  {
367  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
368  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
369  m_scratch_v.resize(bod->getNumLinks() + 1);
370  m_scratch_m.resize(bod->getNumLinks() + 1);
371  bool doNotUpdatePos = false;
372  bool isConstraintPass = false;
373  {
374  if (!bod->isUsingRK4Integration())
375  {
377  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
378  getSolverInfo().m_jointFeedbackInWorldSpace,
379  getSolverInfo().m_jointFeedbackInJointFrame);
380  }
381  else
382  {
383  //
384  int numDofs = bod->getNumDofs() + 6;
385  int numPosVars = bod->getNumPosVars() + 7;
387  scratch_r2.resize(2 * numPosVars + 8 * numDofs);
388  //convenience
389  btScalar* pMem = &scratch_r2[0];
390  btScalar* scratch_q0 = pMem;
391  pMem += numPosVars;
392  btScalar* scratch_qx = pMem;
393  pMem += numPosVars;
394  btScalar* scratch_qd0 = pMem;
395  pMem += numDofs;
396  btScalar* scratch_qd1 = pMem;
397  pMem += numDofs;
398  btScalar* scratch_qd2 = pMem;
399  pMem += numDofs;
400  btScalar* scratch_qd3 = pMem;
401  pMem += numDofs;
402  btScalar* scratch_qdd0 = pMem;
403  pMem += numDofs;
404  btScalar* scratch_qdd1 = pMem;
405  pMem += numDofs;
406  btScalar* scratch_qdd2 = pMem;
407  pMem += numDofs;
408  btScalar* scratch_qdd3 = pMem;
409  pMem += numDofs;
410  btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
411 
413  //copy q0 to scratch_q0 and qd0 to scratch_qd0
414  scratch_q0[0] = bod->getWorldToBaseRot().x();
415  scratch_q0[1] = bod->getWorldToBaseRot().y();
416  scratch_q0[2] = bod->getWorldToBaseRot().z();
417  scratch_q0[3] = bod->getWorldToBaseRot().w();
418  scratch_q0[4] = bod->getBasePos().x();
419  scratch_q0[5] = bod->getBasePos().y();
420  scratch_q0[6] = bod->getBasePos().z();
421  //
422  for (int link = 0; link < bod->getNumLinks(); ++link)
423  {
424  for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
425  scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
426  }
427  //
428  for (int dof = 0; dof < numDofs; ++dof)
429  scratch_qd0[dof] = bod->getVelocityVector()[dof];
431  struct
432  {
433  btMultiBody* bod;
434  btScalar *scratch_qx, *scratch_q0;
435 
436  void operator()()
437  {
438  for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
439  scratch_qx[dof] = scratch_q0[dof];
440  }
441  } pResetQx = {bod, scratch_qx, scratch_q0};
442  //
443  struct
444  {
445  void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
446  {
447  for (int i = 0; i < size; ++i)
448  pVal[i] = pCurVal[i] + dt * pDer[i];
449  }
450 
451  } pEulerIntegrate;
452  //
453  struct
454  {
455  void operator()(btMultiBody* pBody, const btScalar* pData)
456  {
457  btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
458 
459  for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
460  pVel[i] = pData[i];
461  }
462  } pCopyToVelocityVector;
463  //
464  struct
465  {
466  void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
467  {
468  for (int i = 0; i < size; ++i)
469  pDst[i] = pSrc[start + i];
470  }
471  } pCopy;
472  //
473 
474  btScalar h = solverInfo.m_timeStep;
475 #define output &m_scratch_r[bod->getNumDofs()]
476  //calc qdd0 from: q0 & qd0
478  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
479  getSolverInfo().m_jointFeedbackInJointFrame);
480  pCopy(output, scratch_qdd0, 0, numDofs);
481  //calc q1 = q0 + h/2 * qd0
482  pResetQx();
483  bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
484  //calc qd1 = qd0 + h/2 * qdd0
485  pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
486  //
487  //calc qdd1 from: q1 & qd1
488  pCopyToVelocityVector(bod, scratch_qd1);
490  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
491  getSolverInfo().m_jointFeedbackInJointFrame);
492  pCopy(output, scratch_qdd1, 0, numDofs);
493  //calc q2 = q0 + h/2 * qd1
494  pResetQx();
495  bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
496  //calc qd2 = qd0 + h/2 * qdd1
497  pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
498  //
499  //calc qdd2 from: q2 & qd2
500  pCopyToVelocityVector(bod, scratch_qd2);
502  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
503  getSolverInfo().m_jointFeedbackInJointFrame);
504  pCopy(output, scratch_qdd2, 0, numDofs);
505  //calc q3 = q0 + h * qd2
506  pResetQx();
507  bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
508  //calc qd3 = qd0 + h * qdd2
509  pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
510  //
511  //calc qdd3 from: q3 & qd3
512  pCopyToVelocityVector(bod, scratch_qd3);
514  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
515  getSolverInfo().m_jointFeedbackInJointFrame);
516  pCopy(output, scratch_qdd3, 0, numDofs);
517 
518  //
519  //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
520  //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
522  delta_q.resize(numDofs);
524  delta_qd.resize(numDofs);
525  for (int i = 0; i < numDofs; ++i)
526  {
527  delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
528  delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
529  //delta_q[i] = h*scratch_qd0[i];
530  //delta_qd[i] = h*scratch_qdd0[i];
531  }
532  //
533  pCopyToVelocityVector(bod, scratch_qd0);
534  bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
535  //
536  if (!doNotUpdatePos)
537  {
538  btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
539  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
540 
541  for (int i = 0; i < numDofs; ++i)
542  pRealBuf[i] = delta_q[i];
543 
544  //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
545  bod->setPosUpdated(true);
546  }
547 
548  //ugly hack which resets the cached data to t0 (needed for constraint solver)
549  {
550  for (int link = 0; link < bod->getNumLinks(); ++link)
551  bod->getLink(link).updateCacheMultiDof();
553  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
555  }
556  }
557  }
558 
559 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
560  bod->clearForcesAndTorques();
561 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
562  } //if (!isSleeping)
563  }
564  }
565 }
566 
567 
569 {
572 }
573 
575 {
576  BT_PROFILE("btMultiBody stepPositions");
577  //integrate and update the Featherstone hierarchies
578 
579  for (int b = 0; b < m_multiBodies.size(); b++)
580  {
581  btMultiBody* bod = m_multiBodies[b];
582  bool isSleeping = false;
584  {
585  isSleeping = true;
586  }
587  for (int b = 0; b < bod->getNumLinks(); b++)
588  {
590  isSleeping = true;
591  }
592 
593  if (!isSleeping)
594  {
595  bod->addSplitV();
596  int nLinks = bod->getNumLinks();
597 
599  if (!bod->isPosUpdated())
600  bod->stepPositionsMultiDof(timeStep);
601  else
602  {
603  btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
604  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
605 
606  bod->stepPositionsMultiDof(1, 0, pRealBuf);
607  bod->setPosUpdated(false);
608  }
609 
610 
611  m_scratch_world_to_local.resize(nLinks + 1);
612  m_scratch_local_origin.resize(nLinks + 1);
614  bod->substractSplitV();
615  }
616  else
617  {
618  bod->clearVelocities();
619  }
620  }
621 }
622 
624 {
625  BT_PROFILE("btMultiBody stepPositions");
626  //integrate and update the Featherstone hierarchies
627 
628  for (int b = 0; b < m_multiBodies.size(); b++)
629  {
630  btMultiBody* bod = m_multiBodies[b];
631  bool isSleeping = false;
633  {
634  isSleeping = true;
635  }
636  for (int b = 0; b < bod->getNumLinks(); b++)
637  {
639  isSleeping = true;
640  }
641 
642  if (!isSleeping)
643  {
644  int nLinks = bod->getNumLinks();
645  bod->predictPositionsMultiDof(timeStep);
646  m_scratch_world_to_local.resize(nLinks + 1);
647  m_scratch_local_origin.resize(nLinks + 1);
649  }
650  else
651  {
652  bod->clearVelocities();
653  }
654  }
655 }
656 
658 {
659  m_multiBodyConstraints.push_back(constraint);
660 }
661 
663 {
664  m_multiBodyConstraints.remove(constraint);
665 }
666 
668 {
669  constraint->debugDraw(getDebugDrawer());
670 }
671 
673 {
674  BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
675 
677 
678  bool drawConstraints = false;
679  if (getDebugDrawer())
680  {
681  int mode = getDebugDrawer()->getDebugMode();
683  {
684  drawConstraints = true;
685  }
686 
687  if (drawConstraints)
688  {
689  BT_PROFILE("btMultiBody debugDrawWorld");
690 
691  for (int c = 0; c < m_multiBodyConstraints.size(); c++)
692  {
694  debugDrawMultiBodyConstraint(constraint);
695  }
696 
697  for (int b = 0; b < m_multiBodies.size(); b++)
698  {
699  btMultiBody* bod = m_multiBodies[b];
701 
702  if (mode & btIDebugDraw::DBG_DrawFrames)
703  {
705  }
706 
707  for (int m = 0; m < bod->getNumLinks(); m++)
708  {
709  const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
710  if (mode & btIDebugDraw::DBG_DrawFrames)
711  {
712  getDebugDrawer()->drawTransform(tr, 0.1);
713  }
714  //draw the joint axis
716  {
717  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
718 
719  btVector4 color(0, 0, 0, 1); //1,1,1);
720  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
721  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
722  getDebugDrawer()->drawLine(from, to, color);
723  }
725  {
726  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
727 
728  btVector4 color(0, 0, 0, 1); //1,1,1);
729  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
730  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
731  getDebugDrawer()->drawLine(from, to, color);
732  }
734  {
735  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
736 
737  btVector4 color(0, 0, 0, 1); //1,1,1);
738  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
739  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
740  getDebugDrawer()->drawLine(from, to, color);
741  }
742  }
743  }
744  }
745  }
746 }
747 
749 {
751 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
752  BT_PROFILE("btMultiBody addGravity");
753  for (int i = 0; i < this->m_multiBodies.size(); i++)
754  {
755  btMultiBody* bod = m_multiBodies[i];
756 
757  bool isSleeping = false;
758 
760  {
761  isSleeping = true;
762  }
763  for (int b = 0; b < bod->getNumLinks(); b++)
764  {
766  isSleeping = true;
767  }
768 
769  if (!isSleeping)
770  {
771  bod->addBaseForce(m_gravity * bod->getBaseMass());
772 
773  for (int j = 0; j < bod->getNumLinks(); ++j)
774  {
775  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
776  }
777  } //if (!isSleeping)
778  }
779 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
780 }
781 
783 {
784  for (int i = 0; i < this->m_multiBodies.size(); i++)
785  {
786  btMultiBody* bod = m_multiBodies[i];
787  bod->clearConstraintForces();
788  }
789 }
791 {
792  {
793  // BT_PROFILE("clearMultiBodyForces");
794  for (int i = 0; i < this->m_multiBodies.size(); i++)
795  {
796  btMultiBody* bod = m_multiBodies[i];
797 
798  bool isSleeping = false;
799 
801  {
802  isSleeping = true;
803  }
804  for (int b = 0; b < bod->getNumLinks(); b++)
805  {
807  isSleeping = true;
808  }
809 
810  if (!isSleeping)
811  {
812  btMultiBody* bod = m_multiBodies[i];
813  bod->clearForcesAndTorques();
814  }
815  }
816  }
817 }
819 {
821 
822 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
824 #endif
825 }
826 
828 {
829  serializer->startSerialization();
830 
831  serializeDynamicsWorldInfo(serializer);
832 
833  serializeMultiBodies(serializer);
834 
835  serializeRigidBodies(serializer);
836 
837  serializeCollisionObjects(serializer);
838 
839  serializeContactManifolds(serializer);
840 
841  serializer->finishSerialization();
842 }
843 
845 {
846  int i;
847  //serialize all collision objects
848  for (i = 0; i < m_multiBodies.size(); i++)
849  {
850  btMultiBody* mb = m_multiBodies[i];
851  {
852  int len = mb->calculateSerializeBufferSize();
853  btChunk* chunk = serializer->allocate(len, 1);
854  const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
855  serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
856  }
857  }
858 
859  //serialize all multibody links (collision objects)
860  for (i = 0; i < m_collisionObjects.size(); i++)
861  {
864  {
865  int len = colObj->calculateSerializeBufferSize();
866  btChunk* chunk = serializer->allocate(len, 1);
867  const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
868  serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
869  }
870  }
871 }
872 
874 {
876  for(int i = 0; i < m_multiBodies.size(); i++)
877  {
878  btMultiBody* body = m_multiBodies[i];
879  if(body->isBaseKinematic())
880  body->saveKinematicState(timeStep);
881  }
882 }
883 
884 //
885 //void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
886 //{
887 // m_islandManager->setSplitIslands(split);
888 //}
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
@ BT_MULTIBODY_SOLVER
@ SOLVER_USE_2_FRICTION_DIRECTIONS
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
#define output
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:153
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:108
#define BT_MB_LINKCOLLIDER_CODE
Definition: btSerializer.h:109
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void remove(const T &key)
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size,...
btCollisionObject can be used to manage collision detection objects.
bool isStaticOrKinematicObject() const
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
int getInternalType() const
reserved for Bullet internal usage
void setActivationState(int newState) const
virtual int calculateSerializeBufferSize() const
int getIslandTag() const
void setDeactivationTime(btScalar time)
int getActivationState() const
btAlignedObjectArray< btCollisionObject * > m_collisionObjects
btDispatcher * getDispatcher()
virtual btIDebugDraw * getDebugDrawer()
int getNumCollisionObjects() const
btIDebugDraw * m_debugDrawer
void serializeContactManifolds(btSerializer *serializer)
void serializeCollisionObjects(btSerializer *serializer)
virtual btConstraintSolverType getSolverType() const =0
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
virtual void prepareSolve(int, int)
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
btSimulationIslandManager * getSimulationIslandManager()
virtual void integrateTransforms(btScalar timeStep)
void serializeRigidBodies(btSerializer *serializer)
void serializeDynamicsWorldInfo(btSerializer *serializer)
btCollisionWorld * getCollisionWorld()
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
virtual void setConstraintSolver(btConstraintSolver *solver)
virtual void saveKinematicState(btScalar timeStep)
virtual void applyGravity()
apply gravity, call this once per timestep
btSimulationIslandManager * m_islandManager
btAlignedObjectArray< btTypedConstraint * > m_constraints
virtual void updateActivationState(btScalar timeStep)
btConstraintSolver * m_constraintSolver
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
virtual void predictUnconstraintMotion(btScalar timeStep)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btContactSolverInfo & getSolverInfo()
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
virtual int getDebugMode() const =0
@ DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:67
virtual int getIslandIdA() const =0
virtual void debugDraw(class btIDebugDraw *drawer)=0
virtual int getIslandIdB() const =0
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
virtual void solveInternalConstraints(btContactSolverInfo &solverInfo)
virtual void updateActivationState(btScalar timeStep)
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local1
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
virtual void serialize(btSerializer *serializer)
Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see B...
btAlignedObjectArray< btVector3 > m_scratch_local_origin1
virtual void serializeMultiBodies(btSerializer *serializer)
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
btAlignedObjectArray< btVector3 > m_scratch_v
virtual void predictUnconstraintMotion(btScalar timeStep)
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
void predictMultiBodyTransforms(btScalar timeStep)
virtual void integrateTransforms(btScalar timeStep)
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
btAlignedObjectArray< btMultiBody * > m_multiBodies
virtual void getAnalyticsData(btAlignedObjectArray< struct btSolverAnalyticsData > &m_islandAnalyticsData) const
virtual void solveConstraints(btContactSolverInfo &solverInfo)
void integrateMultiBodyTransforms(btScalar timeStep)
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
virtual void solveExternalForces(btContactSolverInfo &solverInfo)
virtual void setConstraintSolver(btConstraintSolver *solver)
btAlignedObjectArray< btVector3 > m_scratch_local_origin
btAlignedObjectArray< btScalar > m_scratch_r
virtual void removeMultiBody(btMultiBody *body)
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
virtual void addMultiBody(btMultiBody *body, int group=btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter)
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
virtual void saveKinematicState(btScalar timeStep)
virtual void applyGravity()
apply gravity, call this once per timestep
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
bool isAwake() const
Definition: btMultiBody.h:548
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void predictPositionsMultiDof(btScalar dt)
void clearVelocities()
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
int getNumLinks() const
Definition: btMultiBody.h:166
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:193
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
void checkMotionAndSleepIfRequired(btScalar timestep)
bool isBaseKinematic() const
btScalar getLinkMass(int i) const
int getNumDofs() const
Definition: btMultiBody.h:167
void addLinkForce(int i, const btVector3 &f)
void addSplitV()
Definition: btMultiBody.h:442
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:455
void clearConstraintForces()
void setPosUpdated(bool updated)
Definition: btMultiBody.h:648
bool isUsingRK4Integration() const
Definition: btMultiBody.h:640
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:654
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:302
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:228
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btScalar getBaseMass() const
Definition: btMultiBody.h:169
int getNumPosVars() const
Definition: btMultiBody.h:168
virtual int calculateSerializeBufferSize() const
void substractSplitV()
Definition: btMultiBody.h:446
bool isPosUpdated() const
Definition: btMultiBody.h:644
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:465
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:355
void saveKinematicState(btScalar timeStep)
void clearForcesAndTorques()
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * getBody1() const
const btCollisionObject * getBody0() const
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void finishSerialization()=0
virtual void startSerialization()=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
virtual void storeIslandActivationState(btCollisionWorld *world)
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
TypedConstraint is the baseclass for Bullet constraints and vehicles.
const btRigidBody & getRigidBodyA() const
bool isEnabled() const
const btRigidBody & getRigidBodyB() const
void unite(int p, int q)
Definition: btUnionFind.h:76
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
virtual void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
btAlignedObjectArray< btSolverAnalyticsData > m_islandAnalyticsData