Blender  V3.3
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 
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  {
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  {
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  {
90  btMultiBodyLinkCollider* prev = body->getBaseCollider();
91 
92  for (int b = 0; b < body->getNumLinks(); b++)
93  {
94  btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
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  {
137  btMultiBodyLinkCollider* col = body->getBaseCollider();
138  if (col && col->getActivationState() == ACTIVE_TAG)
139  {
140  col->setActivationState(WANTS_DEACTIVATION);
141  col->setDeactivationTime(0.f);
142  }
143  for (int b = 0; b < body->getNumLinks(); b++)
144  {
145  btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
146  if (col && col->getActivationState() == ACTIVE_TAG)
147  {
148  col->setActivationState(WANTS_DEACTIVATION);
149  col->setDeactivationTime(0.f);
150  }
151  }
152  }
153  else
154  {
155  btMultiBodyLinkCollider* col = body->getBaseCollider();
156  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
157  col->setActivationState(ACTIVE_TAG);
158 
159  for (int b = 0; b < body->getNumLinks(); b++)
160  {
161  btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
162  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
163  col->setActivationState(ACTIVE_TAG);
164  }
165  }
166  }
167  }
168 
170 }
171 
173 {
175 }
176 
177 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
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;
183  getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
184  m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
185 }
186 
188 {
190 }
191 
192 void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
193 {
197 }
198 
200 {
201  if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
202  {
203  m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
204  }
206 }
207 
209 {
210  for (int b = 0; b < m_multiBodies.size(); b++)
211  {
212  btMultiBody* bod = m_multiBodies[b];
213  bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin);
214  }
215 }
217 {
218  solveExternalForces(solverInfo);
219  buildIslands();
220  solveInternalConstraints(solverInfo);
221 }
222 
224 {
226 }
227 
229 {
232  m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
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 
241  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
242  {
243  isSleeping = true;
244  }
245  for (int b = 0; b < bod->getNumLinks(); b++)
246  {
247  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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;
265  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
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];
277  bod->processDeltaVeeMultiDof2();
278  }
279 }
280 
282 {
284 
285  BT_PROFILE("solveConstraints");
286 
288 
289  m_sortedConstraints.resize(m_constraints.size());
290  int i;
291  for (i = 0; i < getNumConstraints(); i++)
292  {
293  m_sortedConstraints[i] = m_constraints[i];
294  }
295  m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
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());
308  m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
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 
319  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
320  {
321  isSleeping = true;
322  }
323  for (int b = 0; b < bod->getNumLinks(); b++)
324  {
325  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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 
355  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
356  {
357  isSleeping = true;
358  }
359  for (int b = 0; b < bod->getNumLinks(); b++)
360  {
361  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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  {
376  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
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
477  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
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);
489  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
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);
501  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
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);
513  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
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();
552  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
553  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
554  getSolverInfo().m_jointFeedbackInJointFrame);
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;
583  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
584  {
585  isSleeping = true;
586  }
587  for (int b = 0; b < bod->getNumLinks(); b++)
588  {
589  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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);
613  bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
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;
632  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
633  {
634  isSleeping = true;
635  }
636  for (int b = 0; b < bod->getNumLinks(); b++)
637  {
638  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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);
648  bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
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];
700  bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1);
701 
702  if (mode & btIDebugDraw::DBG_DrawFrames)
703  {
704  getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
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
715  if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute)
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  }
724  if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed)
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  }
733  if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic)
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 
759  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
760  {
761  isSleeping = true;
762  }
763  for (int b = 0; b < bod->getNumLinks(); b++)
764  {
765  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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 
800  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
801  {
802  isSleeping = true;
803  }
804  for (int b = 0; b < bod->getNumLinks(); b++)
805  {
806  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
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  {
862  btCollisionObject* colObj = m_collisionObjects[i];
863  if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
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 //
873 //void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
874 //{
875 // m_islandManager->setSplitIslands(split);
876 //}
Group Output data from inside of a node group A color picker Mix two input colors RGB to Convert a color s luminance to a grayscale value Generate a normal vector and a dot product Bright Control the brightness and contrast of the input color Vector Map an input vectors to used to fine tune the interpolation of the input Camera Retrieve information about the camera and how it relates to the current shading point s position Clamp a value between a minimum and a maximum Vector Perform vector math operation Invert a color
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
btCollisionObject
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
@ CO_FEATHERSTONE_LINK
#define WANTS_DEACTIVATION
SIMD_FORCE_INLINE int getIslandTag() const
#define ISLAND_SLEEPING
@ BT_MULTIBODY_SOLVER
@ SOLVER_USE_2_FRICTION_DIRECTIONS
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
btSimulationIslandManager * m_islandManager
void serializeRigidBodies(btSerializer *serializer)
void serializeDynamicsWorldInfo(btSerializer *serializer)
virtual void updateActivationState(btScalar timeStep)
btDiscreteDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete thos...
virtual int getNumConstraints() const
virtual void debugDrawWorld()
virtual void predictUnconstraintMotion(btScalar timeStep)
btAlignedObjectArray< btTypedConstraint * > m_constraints
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
virtual void integrateTransforms(btScalar timeStep)
btSimulationIslandManager * getSimulationIslandManager()
btCollisionWorld * getCollisionWorld()
virtual void setConstraintSolver(btConstraintSolver *solver)
btConstraintSolver * m_constraintSolver
virtual void applyGravity()
apply gravity, call this once per timestep
btScalar m_gravity
btMultiBodyConstraint
#define output
btMultiBody
Definition: btMultiBody.h:51
btPersistentManifold()
SIMD_FORCE_INLINE 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:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:108
#define BT_MB_LINKCOLLIDER_CODE
Definition: btSerializer.h:109
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition: btTransform.h:90
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
void remove(const T &key)
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
SIMD_FORCE_INLINE void push_back(const T &_Val)
void * m_oldPtr
Definition: btSerializer.h:52
virtual btConstraintSolverType getSolverType() const =0
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
virtual void prepareSolve(int, int)
@ DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:67
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
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)
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 setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
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)
void unite(int p, int q)
Definition: btUnionFind.h:76
StackEntry * from
int len
Definition: draw_manager.c:108
uint col
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
Definition: math_float4.h:513
static unsigned c
Definition: RandGen.cpp:83
SymEdge< T > * prev(const SymEdge< T > *se)
Definition: delaunay_2d.cc:105
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
virtual SIMD_FORCE_INLINE void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
btAlignedObjectArray< btSolverAnalyticsData > m_islandAnalyticsData