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  {
140  if (body->hasFixedBase())
141  {
143  } else
144  {
146  }
147 
148  col->setDeactivationTime(0.f);
149  }
150  for (int b = 0; b < body->getNumLinks(); b++)
151  {
153  if (col && col->getActivationState() == ACTIVE_TAG)
154  {
156  col->setDeactivationTime(0.f);
157  }
158  }
159  }
160  else
161  {
163  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
165 
166  for (int b = 0; b < body->getNumLinks(); b++)
167  {
169  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
171  }
172  }
173  }
174  }
175 
177 }
178 
180 {
182 }
183 
185  : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
186  m_multiBodyConstraintSolver(constraintSolver)
187 {
188  //split impulse is not yet supported for Featherstone hierarchies
189  // getSolverInfo().m_splitImpulse = false;
191  m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
192 }
193 
195 {
197 }
198 
200 {
204 }
205 
207 {
208  if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
209  {
211  }
213 }
214 
216 {
217  for (int b = 0; b < m_multiBodies.size(); b++)
218  {
219  btMultiBody* bod = m_multiBodies[b];
221  }
222 }
224 {
225  solveExternalForces(solverInfo);
226  buildIslands();
227  solveInternalConstraints(solverInfo);
228 }
229 
231 {
233 }
234 
236 {
240  {
241  BT_PROFILE("btMultiBody stepVelocities");
242  for (int i = 0; i < this->m_multiBodies.size(); i++)
243  {
244  btMultiBody* bod = m_multiBodies[i];
245 
246  bool isSleeping = false;
247 
249  {
250  isSleeping = true;
251  }
252  for (int b = 0; b < bod->getNumLinks(); b++)
253  {
255  isSleeping = true;
256  }
257 
258  if (!isSleeping)
259  {
260  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
261  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
262  m_scratch_v.resize(bod->getNumLinks() + 1);
263  m_scratch_m.resize(bod->getNumLinks() + 1);
264 
265  if (bod->internalNeedsJointFeedback())
266  {
267  if (!bod->isUsingRK4Integration())
268  {
269  if (bod->internalNeedsJointFeedback())
270  {
271  bool isConstraintPass = true;
273  getSolverInfo().m_jointFeedbackInWorldSpace,
274  getSolverInfo().m_jointFeedbackInJointFrame);
275  }
276  }
277  }
278  }
279  }
280  }
281  for (int i = 0; i < this->m_multiBodies.size(); i++)
282  {
283  btMultiBody* bod = m_multiBodies[i];
285  }
286 }
287 
289 {
291 
292  BT_PROFILE("solveConstraints");
293 
295 
297  int i;
298  for (i = 0; i < getNumConstraints(); i++)
299  {
301  }
303  btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
304 
306  for (i = 0; i < m_multiBodyConstraints.size(); i++)
307  {
309  }
311 
312  btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
313 
314  m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
316 
317 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
318  {
319  BT_PROFILE("btMultiBody addForce");
320  for (int i = 0; i < this->m_multiBodies.size(); i++)
321  {
322  btMultiBody* bod = m_multiBodies[i];
323 
324  bool isSleeping = false;
325 
327  {
328  isSleeping = true;
329  }
330  for (int b = 0; b < bod->getNumLinks(); b++)
331  {
333  isSleeping = true;
334  }
335 
336  if (!isSleeping)
337  {
338  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
339  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
340  m_scratch_v.resize(bod->getNumLinks() + 1);
341  m_scratch_m.resize(bod->getNumLinks() + 1);
342 
343  bod->addBaseForce(m_gravity * bod->getBaseMass());
344 
345  for (int j = 0; j < bod->getNumLinks(); ++j)
346  {
347  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
348  }
349  } //if (!isSleeping)
350  }
351  }
352 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
353 
354  {
355  BT_PROFILE("btMultiBody stepVelocities");
356  for (int i = 0; i < this->m_multiBodies.size(); i++)
357  {
358  btMultiBody* bod = m_multiBodies[i];
359 
360  bool isSleeping = false;
361 
363  {
364  isSleeping = true;
365  }
366  for (int b = 0; b < bod->getNumLinks(); b++)
367  {
369  isSleeping = true;
370  }
371 
372  if (!isSleeping)
373  {
374  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
375  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
376  m_scratch_v.resize(bod->getNumLinks() + 1);
377  m_scratch_m.resize(bod->getNumLinks() + 1);
378  bool doNotUpdatePos = false;
379  bool isConstraintPass = false;
380  {
381  if (!bod->isUsingRK4Integration())
382  {
384  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
385  getSolverInfo().m_jointFeedbackInWorldSpace,
386  getSolverInfo().m_jointFeedbackInJointFrame);
387  }
388  else
389  {
390  //
391  int numDofs = bod->getNumDofs() + 6;
392  int numPosVars = bod->getNumPosVars() + 7;
394  scratch_r2.resize(2 * numPosVars + 8 * numDofs);
395  //convenience
396  btScalar* pMem = &scratch_r2[0];
397  btScalar* scratch_q0 = pMem;
398  pMem += numPosVars;
399  btScalar* scratch_qx = pMem;
400  pMem += numPosVars;
401  btScalar* scratch_qd0 = pMem;
402  pMem += numDofs;
403  btScalar* scratch_qd1 = pMem;
404  pMem += numDofs;
405  btScalar* scratch_qd2 = pMem;
406  pMem += numDofs;
407  btScalar* scratch_qd3 = pMem;
408  pMem += numDofs;
409  btScalar* scratch_qdd0 = pMem;
410  pMem += numDofs;
411  btScalar* scratch_qdd1 = pMem;
412  pMem += numDofs;
413  btScalar* scratch_qdd2 = pMem;
414  pMem += numDofs;
415  btScalar* scratch_qdd3 = pMem;
416  pMem += numDofs;
417  btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
418 
420  //copy q0 to scratch_q0 and qd0 to scratch_qd0
421  scratch_q0[0] = bod->getWorldToBaseRot().x();
422  scratch_q0[1] = bod->getWorldToBaseRot().y();
423  scratch_q0[2] = bod->getWorldToBaseRot().z();
424  scratch_q0[3] = bod->getWorldToBaseRot().w();
425  scratch_q0[4] = bod->getBasePos().x();
426  scratch_q0[5] = bod->getBasePos().y();
427  scratch_q0[6] = bod->getBasePos().z();
428  //
429  for (int link = 0; link < bod->getNumLinks(); ++link)
430  {
431  for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
432  scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
433  }
434  //
435  for (int dof = 0; dof < numDofs; ++dof)
436  scratch_qd0[dof] = bod->getVelocityVector()[dof];
438  struct
439  {
440  btMultiBody* bod;
441  btScalar *scratch_qx, *scratch_q0;
442 
443  void operator()()
444  {
445  for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
446  scratch_qx[dof] = scratch_q0[dof];
447  }
448  } pResetQx = {bod, scratch_qx, scratch_q0};
449  //
450  struct
451  {
452  void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
453  {
454  for (int i = 0; i < size; ++i)
455  pVal[i] = pCurVal[i] + dt * pDer[i];
456  }
457 
458  } pEulerIntegrate;
459  //
460  struct
461  {
462  void operator()(btMultiBody* pBody, const btScalar* pData)
463  {
464  btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
465 
466  for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
467  pVel[i] = pData[i];
468  }
469  } pCopyToVelocityVector;
470  //
471  struct
472  {
473  void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
474  {
475  for (int i = 0; i < size; ++i)
476  pDst[i] = pSrc[start + i];
477  }
478  } pCopy;
479  //
480 
481  btScalar h = solverInfo.m_timeStep;
482 #define output &m_scratch_r[bod->getNumDofs()]
483  //calc qdd0 from: q0 & qd0
485  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
486  getSolverInfo().m_jointFeedbackInJointFrame);
487  pCopy(output, scratch_qdd0, 0, numDofs);
488  //calc q1 = q0 + h/2 * qd0
489  pResetQx();
490  bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
491  //calc qd1 = qd0 + h/2 * qdd0
492  pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
493  //
494  //calc qdd1 from: q1 & qd1
495  pCopyToVelocityVector(bod, scratch_qd1);
497  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
498  getSolverInfo().m_jointFeedbackInJointFrame);
499  pCopy(output, scratch_qdd1, 0, numDofs);
500  //calc q2 = q0 + h/2 * qd1
501  pResetQx();
502  bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
503  //calc qd2 = qd0 + h/2 * qdd1
504  pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
505  //
506  //calc qdd2 from: q2 & qd2
507  pCopyToVelocityVector(bod, scratch_qd2);
509  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
510  getSolverInfo().m_jointFeedbackInJointFrame);
511  pCopy(output, scratch_qdd2, 0, numDofs);
512  //calc q3 = q0 + h * qd2
513  pResetQx();
514  bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
515  //calc qd3 = qd0 + h * qdd2
516  pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
517  //
518  //calc qdd3 from: q3 & qd3
519  pCopyToVelocityVector(bod, scratch_qd3);
521  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
522  getSolverInfo().m_jointFeedbackInJointFrame);
523  pCopy(output, scratch_qdd3, 0, numDofs);
524 
525  //
526  //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
527  //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
529  delta_q.resize(numDofs);
531  delta_qd.resize(numDofs);
532  for (int i = 0; i < numDofs; ++i)
533  {
534  delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
535  delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
536  //delta_q[i] = h*scratch_qd0[i];
537  //delta_qd[i] = h*scratch_qdd0[i];
538  }
539  //
540  pCopyToVelocityVector(bod, scratch_qd0);
541  bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
542  //
543  if (!doNotUpdatePos)
544  {
545  btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
546  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
547 
548  for (int i = 0; i < numDofs; ++i)
549  pRealBuf[i] = delta_q[i];
550 
551  //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
552  bod->setPosUpdated(true);
553  }
554 
555  //ugly hack which resets the cached data to t0 (needed for constraint solver)
556  {
557  for (int link = 0; link < bod->getNumLinks(); ++link)
558  bod->getLink(link).updateCacheMultiDof();
560  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
562  }
563  }
564  }
565 
566 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
567  bod->clearForcesAndTorques();
568 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
569  } //if (!isSleeping)
570  }
571  }
572 }
573 
574 
576 {
579 }
580 
582 {
583  BT_PROFILE("btMultiBody stepPositions");
584  //integrate and update the Featherstone hierarchies
585 
586  for (int b = 0; b < m_multiBodies.size(); b++)
587  {
588  btMultiBody* bod = m_multiBodies[b];
589  bool isSleeping = false;
591  {
592  isSleeping = true;
593  }
594  for (int b = 0; b < bod->getNumLinks(); b++)
595  {
597  isSleeping = true;
598  }
599 
600  if (!isSleeping)
601  {
602  bod->addSplitV();
603  int nLinks = bod->getNumLinks();
604 
606  if (!bod->isPosUpdated())
607  bod->stepPositionsMultiDof(timeStep);
608  else
609  {
610  btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
611  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
612 
613  bod->stepPositionsMultiDof(1, 0, pRealBuf);
614  bod->setPosUpdated(false);
615  }
616 
617 
618  m_scratch_world_to_local.resize(nLinks + 1);
619  m_scratch_local_origin.resize(nLinks + 1);
621  bod->substractSplitV();
622  }
623  else
624  {
625  bod->clearVelocities();
626  }
627  }
628 }
629 
631 {
632  BT_PROFILE("btMultiBody stepPositions");
633  //integrate and update the Featherstone hierarchies
634 
635  for (int b = 0; b < m_multiBodies.size(); b++)
636  {
637  btMultiBody* bod = m_multiBodies[b];
638  bool isSleeping = false;
640  {
641  isSleeping = true;
642  }
643  for (int b = 0; b < bod->getNumLinks(); b++)
644  {
646  isSleeping = true;
647  }
648 
649  if (!isSleeping)
650  {
651  int nLinks = bod->getNumLinks();
652  bod->predictPositionsMultiDof(timeStep);
653  m_scratch_world_to_local.resize(nLinks + 1);
654  m_scratch_local_origin.resize(nLinks + 1);
656  }
657  else
658  {
659  bod->clearVelocities();
660  }
661  }
662 }
663 
665 {
666  m_multiBodyConstraints.push_back(constraint);
667 }
668 
670 {
671  m_multiBodyConstraints.remove(constraint);
672 }
673 
675 {
676  constraint->debugDraw(getDebugDrawer());
677 }
678 
680 {
681  BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
682 
684 
685  bool drawConstraints = false;
686  if (getDebugDrawer())
687  {
688  int mode = getDebugDrawer()->getDebugMode();
690  {
691  drawConstraints = true;
692  }
693 
694  if (drawConstraints)
695  {
696  BT_PROFILE("btMultiBody debugDrawWorld");
697 
698  for (int c = 0; c < m_multiBodyConstraints.size(); c++)
699  {
701  debugDrawMultiBodyConstraint(constraint);
702  }
703 
704  for (int b = 0; b < m_multiBodies.size(); b++)
705  {
706  btMultiBody* bod = m_multiBodies[b];
708 
709  if (mode & btIDebugDraw::DBG_DrawFrames)
710  {
712  }
713 
714  for (int m = 0; m < bod->getNumLinks(); m++)
715  {
716  const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
717  if (mode & btIDebugDraw::DBG_DrawFrames)
718  {
719  getDebugDrawer()->drawTransform(tr, 0.1);
720  }
721  //draw the joint axis
723  {
724  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
725 
726  btVector4 color(0, 0, 0, 1); //1,1,1);
727  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
728  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
729  getDebugDrawer()->drawLine(from, to, color);
730  }
732  {
733  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
734 
735  btVector4 color(0, 0, 0, 1); //1,1,1);
736  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
737  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
738  getDebugDrawer()->drawLine(from, to, color);
739  }
741  {
742  btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
743 
744  btVector4 color(0, 0, 0, 1); //1,1,1);
745  btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
746  btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
747  getDebugDrawer()->drawLine(from, to, color);
748  }
749  }
750  }
751  }
752  }
753 }
754 
756 {
758 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
759  BT_PROFILE("btMultiBody addGravity");
760  for (int i = 0; i < this->m_multiBodies.size(); i++)
761  {
762  btMultiBody* bod = m_multiBodies[i];
763 
764  bool isSleeping = false;
765 
767  {
768  isSleeping = true;
769  }
770  for (int b = 0; b < bod->getNumLinks(); b++)
771  {
773  isSleeping = true;
774  }
775 
776  if (!isSleeping)
777  {
778  bod->addBaseForce(m_gravity * bod->getBaseMass());
779 
780  for (int j = 0; j < bod->getNumLinks(); ++j)
781  {
782  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
783  }
784  } //if (!isSleeping)
785  }
786 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
787 }
788 
790 {
791  for (int i = 0; i < this->m_multiBodies.size(); i++)
792  {
793  btMultiBody* bod = m_multiBodies[i];
794  bod->clearConstraintForces();
795  }
796 }
798 {
799  {
800  // BT_PROFILE("clearMultiBodyForces");
801  for (int i = 0; i < this->m_multiBodies.size(); i++)
802  {
803  btMultiBody* bod = m_multiBodies[i];
804 
805  bool isSleeping = false;
806 
808  {
809  isSleeping = true;
810  }
811  for (int b = 0; b < bod->getNumLinks(); b++)
812  {
814  isSleeping = true;
815  }
816 
817  if (!isSleeping)
818  {
819  btMultiBody* bod = m_multiBodies[i];
820  bod->clearForcesAndTorques();
821  }
822  }
823  }
824 }
826 {
828 
829 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
831 #endif
832 }
833 
835 {
836  serializer->startSerialization();
837 
838  serializeDynamicsWorldInfo(serializer);
839 
840  serializeMultiBodies(serializer);
841 
842  serializeRigidBodies(serializer);
843 
844  serializeCollisionObjects(serializer);
845 
846  serializeContactManifolds(serializer);
847 
848  serializer->finishSerialization();
849 }
850 
852 {
853  int i;
854  //serialize all collision objects
855  for (i = 0; i < m_multiBodies.size(); i++)
856  {
857  btMultiBody* mb = m_multiBodies[i];
858  {
859  int len = mb->calculateSerializeBufferSize();
860  btChunk* chunk = serializer->allocate(len, 1);
861  const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
862  serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
863  }
864  }
865 
866  //serialize all multibody links (collision objects)
867  for (i = 0; i < m_collisionObjects.size(); i++)
868  {
871  {
872  int len = colObj->calculateSerializeBufferSize();
873  btChunk* chunk = serializer->allocate(len, 1);
874  const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
875  serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
876  }
877  }
878 }
879 
881 {
883  for(int i = 0; i < m_multiBodies.size(); i++)
884  {
885  btMultiBody* body = m_multiBodies[i];
886  if(body->isBaseKinematic())
887  body->saveKinematicState(timeStep);
888  }
889 }
890 
891 //
892 //void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
893 //{
894 // m_islandManager->setSplitIslands(split);
895 //}
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define FIXED_BASE_MULTI_BODY
#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
bool hasFixedBase() const
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:455
void clearConstraintForces()
void setPosUpdated(bool updated)
Definition: btMultiBody.h:651
bool isUsingRK4Integration() const
Definition: btMultiBody.h:643
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:657
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:647
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