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"
24 
25 
26 
27 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
28 {
30 
31 }
32 
34 {
35  m_multiBodies.remove(body);
36 }
37 
39 {
40  BT_PROFILE("calculateSimulationIslands");
41 
43 
44  {
45  //merge islands based on speculative contact manifolds too
46  for (int i=0;i<this->m_predictiveManifolds.size();i++)
47  {
49 
50  const btCollisionObject* colObj0 = manifold->getBody0();
51  const btCollisionObject* colObj1 = manifold->getBody1();
52 
53  if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
54  ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
55  {
56  getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
57  }
58  }
59  }
60 
61  {
62  int i;
63  int numConstraints = int(m_constraints.size());
64  for (i=0;i< numConstraints ; i++ )
65  {
66  btTypedConstraint* constraint = m_constraints[i];
67  if (constraint->isEnabled())
68  {
69  const btRigidBody* colObj0 = &constraint->getRigidBodyA();
70  const btRigidBody* colObj1 = &constraint->getRigidBodyB();
71 
72  if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
73  ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
74  {
75  getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
76  }
77  }
78  }
79  }
80 
81  //merge islands linked by Featherstone link colliders
82  for (int i=0;i<m_multiBodies.size();i++)
83  {
84  btMultiBody* body = m_multiBodies[i];
85  {
87 
88  for (int b=0;b<body->getNumLinks();b++)
89  {
91 
92  if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
93  ((prev) && (!(prev)->isStaticOrKinematicObject())))
94  {
95  int tagPrev = prev->getIslandTag();
96  int tagCur = cur->getIslandTag();
97  getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
98  }
99  if (cur && !cur->isStaticOrKinematicObject())
100  prev = cur;
101 
102  }
103  }
104  }
105 
106  //merge islands linked by multibody constraints
107  {
108  for (int i=0;i<this->m_multiBodyConstraints.size();i++)
109  {
111  int tagA = c->getIslandIdA();
112  int tagB = c->getIslandIdB();
113  if (tagA>=0 && tagB>=0)
115  }
116  }
117 
118  //Store the island id in each body
120 
121 }
122 
123 
125 {
126  BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
127 
128 
129 
130  for ( int i=0;i<m_multiBodies.size();i++)
131  {
132  btMultiBody* body = m_multiBodies[i];
133  if (body)
134  {
135  body->checkMotionAndSleepIfRequired(timeStep);
136  if (!body->isAwake())
137  {
139  if (col && col->getActivationState() == ACTIVE_TAG)
140  {
142  col->setDeactivationTime(0.f);
143  }
144  for (int b=0;b<body->getNumLinks();b++)
145  {
147  if (col && col->getActivationState() == ACTIVE_TAG)
148  {
150  col->setDeactivationTime(0.f);
151  }
152  }
153  } else
154  {
156  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
158 
159  for (int b=0;b<body->getNumLinks();b++)
160  {
162  if (col && col->getActivationState() != DISABLE_DEACTIVATION)
164  }
165  }
166 
167  }
168  }
169 
171 }
172 
173 
175 {
176  int islandId;
177 
178  const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
179  const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
180  islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
181  return islandId;
182 
183 }
184 
185 
187 {
188  public:
189 
190  bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
191  {
192  int rIslandId0,lIslandId0;
193  rIslandId0 = btGetConstraintIslandId2(rhs);
194  lIslandId0 = btGetConstraintIslandId2(lhs);
195  return lIslandId0 < rIslandId0;
196  }
197 };
198 
199 
200 
202 {
203  int islandId;
204 
205  int islandTagA = lhs->getIslandIdA();
206  int islandTagB = lhs->getIslandIdB();
207  islandId= islandTagA>=0?islandTagA:islandTagB;
208  return islandId;
209 
210 }
211 
212 
214 {
215  public:
216 
217  bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
218  {
219  int rIslandId0,lIslandId0;
220  rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
221  lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
222  return lIslandId0 < rIslandId0;
223  }
224 };
225 
227 {
232 
237 
242 
243 
245  btDispatcher* dispatcher)
246  :m_solverInfo(NULL),
247  m_solver(solver),
248  m_multiBodySortedConstraints(NULL),
249  m_numConstraints(0),
250  m_debugDrawer(NULL),
251  m_dispatcher(dispatcher)
252  {
253 
254  }
255 
257  {
258  btAssert(0);
259  (void)other;
260  return *this;
261  }
262 
263  SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
264  {
265  btAssert(solverInfo);
266  m_solverInfo = solverInfo;
267 
268  m_multiBodySortedConstraints = sortedMultiBodyConstraints;
269  m_numMultiBodyConstraints = numMultiBodyConstraints;
270  m_sortedConstraints = sortedConstraints;
271  m_numConstraints = numConstraints;
272 
273  m_debugDrawer = debugDrawer;
274  m_bodies.resize (0);
275  m_manifolds.resize (0);
276  m_constraints.resize (0);
277  m_multiBodyConstraints.resize(0);
278  }
279 
280 
281  virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
282  {
283  if (islandId<0)
284  {
286  m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
287  } else
288  {
289  //also add all non-contact constraints/joints for this island
290  btTypedConstraint** startConstraint = 0;
291  btMultiBodyConstraint** startMultiBodyConstraint = 0;
292 
293  int numCurConstraints = 0;
294  int numCurMultiBodyConstraints = 0;
295 
296  int i;
297 
298  //find the first constraint for this island
299 
300  for (i=0;i<m_numConstraints;i++)
301  {
302  if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
303  {
304  startConstraint = &m_sortedConstraints[i];
305  break;
306  }
307  }
308  //count the number of constraints in this island
309  for (;i<m_numConstraints;i++)
310  {
311  if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
312  {
313  numCurConstraints++;
314  }
315  }
316 
317  for (i=0;i<m_numMultiBodyConstraints;i++)
318  {
319  if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
320  {
321 
322  startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
323  break;
324  }
325  }
326  //count the number of multi body constraints in this island
327  for (;i<m_numMultiBodyConstraints;i++)
328  {
329  if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
330  {
331  numCurMultiBodyConstraints++;
332  }
333  }
334 
335  if (m_solverInfo->m_minimumSolverBatchSize<=1)
336  {
337  m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
338  } else
339  {
340 
341  for (i=0;i<numBodies;i++)
342  m_bodies.push_back(bodies[i]);
343  for (i=0;i<numManifolds;i++)
344  m_manifolds.push_back(manifolds[i]);
345  for (i=0;i<numCurConstraints;i++)
346  m_constraints.push_back(startConstraint[i]);
347 
348  for (i=0;i<numCurMultiBodyConstraints;i++)
349  m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
350 
351  if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
352  {
353  processConstraints();
354  } else
355  {
356  //printf("deferred\n");
357  }
358  }
359  }
360  }
362  {
363 
364  btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
365  btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
366  btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
367  btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
368 
369  //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
370 
371  m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
372  m_bodies.resize(0);
373  m_manifolds.resize(0);
374  m_constraints.resize(0);
375  m_multiBodyConstraints.resize(0);
376  }
377 
378 };
379 
380 
381 
383  :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
384  m_multiBodyConstraintSolver(constraintSolver)
385 {
386  //split impulse is not yet supported for Featherstone hierarchies
387  getSolverInfo().m_splitImpulse = false;
390 }
391 
393 {
395 }
396 
398 {
399 
403 
404 
405  BT_PROFILE("solveConstraints");
406 
408  int i;
409  for (i=0;i<getNumConstraints();i++)
410  {
412  }
414  btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
415 
417  for (i=0;i<m_multiBodyConstraints.size();i++)
418  {
420  }
422 
423  btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
424 
425 
426  m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
428 
431 
432 
433  {
434  BT_PROFILE("btMultiBody addForce and stepVelocities");
435  for (int i=0;i<this->m_multiBodies.size();i++)
436  {
437  btMultiBody* bod = m_multiBodies[i];
438 
439  bool isSleeping = false;
440 
442  {
443  isSleeping = true;
444  }
445  for (int b=0;b<bod->getNumLinks();b++)
446  {
448  isSleeping = true;
449  }
450 
451  if (!isSleeping)
452  {
453  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
454  scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
455  scratch_v.resize(bod->getNumLinks()+1);
456  scratch_m.resize(bod->getNumLinks()+1);
457 
458  bod->addBaseForce(m_gravity * bod->getBaseMass());
459 
460  for (int j = 0; j < bod->getNumLinks(); ++j)
461  {
462  bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
463  }
464 
465  bool doNotUpdatePos = false;
466 
467  if(bod->isMultiDof())
468  {
469  if(!bod->isUsingRK4Integration())
470  {
471  bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
472  }
473  else
474  {
475  //
476  int numDofs = bod->getNumDofs() + 6;
477  int numPosVars = bod->getNumPosVars() + 7;
478  btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
479  //convenience
480  btScalar *pMem = &scratch_r2[0];
481  btScalar *scratch_q0 = pMem; pMem += numPosVars;
482  btScalar *scratch_qx = pMem; pMem += numPosVars;
483  btScalar *scratch_qd0 = pMem; pMem += numDofs;
484  btScalar *scratch_qd1 = pMem; pMem += numDofs;
485  btScalar *scratch_qd2 = pMem; pMem += numDofs;
486  btScalar *scratch_qd3 = pMem; pMem += numDofs;
487  btScalar *scratch_qdd0 = pMem; pMem += numDofs;
488  btScalar *scratch_qdd1 = pMem; pMem += numDofs;
489  btScalar *scratch_qdd2 = pMem; pMem += numDofs;
490  btScalar *scratch_qdd3 = pMem; pMem += numDofs;
491  btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
492 
494  //copy q0 to scratch_q0 and qd0 to scratch_qd0
495  scratch_q0[0] = bod->getWorldToBaseRot().x();
496  scratch_q0[1] = bod->getWorldToBaseRot().y();
497  scratch_q0[2] = bod->getWorldToBaseRot().z();
498  scratch_q0[3] = bod->getWorldToBaseRot().w();
499  scratch_q0[4] = bod->getBasePos().x();
500  scratch_q0[5] = bod->getBasePos().y();
501  scratch_q0[6] = bod->getBasePos().z();
502  //
503  for(int link = 0; link < bod->getNumLinks(); ++link)
504  {
505  for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
506  scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
507  }
508  //
509  for(int dof = 0; dof < numDofs; ++dof)
510  scratch_qd0[dof] = bod->getVelocityVector()[dof];
512  struct
513  {
514  btMultiBody *bod;
515  btScalar *scratch_qx, *scratch_q0;
516 
517  void operator()()
518  {
519  for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
520  scratch_qx[dof] = scratch_q0[dof];
521  }
522  } pResetQx = {bod, scratch_qx, scratch_q0};
523  //
524  struct
525  {
526  void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
527  {
528  for(int i = 0; i < size; ++i)
529  pVal[i] = pCurVal[i] + dt * pDer[i];
530  }
531 
532  } pEulerIntegrate;
533  //
534  struct
535  {
536  void operator()(btMultiBody *pBody, const btScalar *pData)
537  {
538  btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
539 
540  for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
541  pVel[i] = pData[i];
542 
543  }
544  } pCopyToVelocityVector;
545  //
546  struct
547  {
548  void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
549  {
550  for(int i = 0; i < size; ++i)
551  pDst[i] = pSrc[start + i];
552  }
553  } pCopy;
554  //
555 
556  btScalar h = solverInfo.m_timeStep;
557  #define output &scratch_r[bod->getNumDofs()]
558  //calc qdd0 from: q0 & qd0
559  bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
560  pCopy(output, scratch_qdd0, 0, numDofs);
561  //calc q1 = q0 + h/2 * qd0
562  pResetQx();
563  bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
564  //calc qd1 = qd0 + h/2 * qdd0
565  pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
566  //
567  //calc qdd1 from: q1 & qd1
568  pCopyToVelocityVector(bod, scratch_qd1);
569  bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
570  pCopy(output, scratch_qdd1, 0, numDofs);
571  //calc q2 = q0 + h/2 * qd1
572  pResetQx();
573  bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
574  //calc qd2 = qd0 + h/2 * qdd1
575  pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
576  //
577  //calc qdd2 from: q2 & qd2
578  pCopyToVelocityVector(bod, scratch_qd2);
579  bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
580  pCopy(output, scratch_qdd2, 0, numDofs);
581  //calc q3 = q0 + h * qd2
582  pResetQx();
583  bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
584  //calc qd3 = qd0 + h * qdd2
585  pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
586  //
587  //calc qdd3 from: q3 & qd3
588  pCopyToVelocityVector(bod, scratch_qd3);
589  bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
590  pCopy(output, scratch_qdd3, 0, numDofs);
591 
592  //
593  //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
594  //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
595  btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
596  btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
597  for(int i = 0; i < numDofs; ++i)
598  {
599  delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
600  delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
601  //delta_q[i] = h*scratch_qd0[i];
602  //delta_qd[i] = h*scratch_qdd0[i];
603  }
604  //
605  pCopyToVelocityVector(bod, scratch_qd0);
606  bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
607  //
608  if(!doNotUpdatePos)
609  {
610  btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
611  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
612 
613  for(int i = 0; i < numDofs; ++i)
614  pRealBuf[i] = delta_q[i];
615 
616  //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
617  bod->setPosUpdated(true);
618  }
619 
620  //ugly hack which resets the cached data to t0 (needed for constraint solver)
621  {
622  for(int link = 0; link < bod->getNumLinks(); ++link)
623  bod->getLink(link).updateCacheMultiDof();
624  bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
625  }
626 
627  }
628  }
629  else//if(bod->isMultiDof())
630  {
631  bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
632  }
633  bod->clearForcesAndTorques();
634  }//if (!isSleeping)
635  }
636  }
637 
639 
641 
642 }
643 
645 {
647 
648  {
649  BT_PROFILE("btMultiBody stepPositions");
650  //integrate and update the Featherstone hierarchies
651  btAlignedObjectArray<btQuaternion> world_to_local;
652  btAlignedObjectArray<btVector3> local_origin;
653 
654  for (int b=0;b<m_multiBodies.size();b++)
655  {
656  btMultiBody* bod = m_multiBodies[b];
657  bool isSleeping = false;
659  {
660  isSleeping = true;
661  }
662  for (int b=0;b<bod->getNumLinks();b++)
663  {
665  isSleeping = true;
666  }
667 
668 
669  if (!isSleeping)
670  {
671  int nLinks = bod->getNumLinks();
672 
674  world_to_local.resize(nLinks+1);
675  local_origin.resize(nLinks+1);
676 
677  if(bod->isMultiDof())
678  {
679  if(!bod->isPosUpdated())
680  bod->stepPositionsMultiDof(timeStep);
681  else
682  {
683  btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
684  pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
685 
686  bod->stepPositionsMultiDof(1, 0, pRealBuf);
687  bod->setPosUpdated(false);
688  }
689  }
690  else
691  bod->stepPositions(timeStep);
692 
693  world_to_local[0] = bod->getWorldToBaseRot();
694  local_origin[0] = bod->getBasePos();
695 
696  if (bod->getBaseCollider())
697  {
698  btVector3 posr = local_origin[0];
699  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
700  btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
701  btTransform tr;
702  tr.setIdentity();
703  tr.setOrigin(posr);
704  tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
705 
707 
708  }
709 
710  for (int k=0;k<bod->getNumLinks();k++)
711  {
712  const int parent = bod->getParent(k);
713  world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
714  local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
715  }
716 
717 
718  for (int m=0;m<bod->getNumLinks();m++)
719  {
721  if (col)
722  {
723  int link = col->m_link;
724  btAssert(link == m);
725 
726  int index = link+1;
727 
728  btVector3 posr = local_origin[index];
729  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
730  btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
731  btTransform tr;
732  tr.setIdentity();
733  tr.setOrigin(posr);
734  tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
735 
736  col->setWorldTransform(tr);
737  }
738  }
739  } else
740  {
741  bod->clearVelocities();
742  }
743  }
744  }
745 }
746 
747 
748 
750 {
751  m_multiBodyConstraints.push_back(constraint);
752 }
753 
755 {
756  m_multiBodyConstraints.remove(constraint);
757 }
758 
760 {
761  constraint->debugDraw(getDebugDrawer());
762 }
763 
764 
766 {
767  BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
768 
769  bool drawConstraints = false;
770  if (getDebugDrawer())
771  {
772  int mode = getDebugDrawer()->getDebugMode();
774  {
775  drawConstraints = true;
776  }
777 
778  if (drawConstraints)
779  {
780  BT_PROFILE("btMultiBody debugDrawWorld");
781 
782  btAlignedObjectArray<btQuaternion> world_to_local;
783  btAlignedObjectArray<btVector3> local_origin;
784 
785  for (int c=0;c<m_multiBodyConstraints.size();c++)
786  {
788  debugDrawMultiBodyConstraint(constraint);
789  }
790 
791  for (int b = 0; b<m_multiBodies.size(); b++)
792  {
793  btMultiBody* bod = m_multiBodies[b];
794  int nLinks = bod->getNumLinks();
795 
797  world_to_local.resize(nLinks + 1);
798  local_origin.resize(nLinks + 1);
799 
800 
801  world_to_local[0] = bod->getWorldToBaseRot();
802  local_origin[0] = bod->getBasePos();
803 
804 
805  {
806  btVector3 posr = local_origin[0];
807  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
808  btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
809  btTransform tr;
810  tr.setIdentity();
811  tr.setOrigin(posr);
812  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
813 
814  getDebugDrawer()->drawTransform(tr, 0.1);
815 
816  }
817 
818  for (int k = 0; k<bod->getNumLinks(); k++)
819  {
820  const int parent = bod->getParent(k);
821  world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
822  local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
823  }
824 
825 
826  for (int m = 0; m<bod->getNumLinks(); m++)
827  {
828  int link = m;
829  int index = link + 1;
830 
831  btVector3 posr = local_origin[index];
832  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
833  btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
834  btTransform tr;
835  tr.setIdentity();
836  tr.setOrigin(posr);
837  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
838 
839  getDebugDrawer()->drawTransform(tr, 0.1);
840  }
841  }
842  }
843  }
844 
846 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
MultiBodyInplaceSolverIslandCallback & operator=(MultiBodyInplaceSolverIslandCallback &other)
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
#define ACTIVE_TAG
btAlignedObjectArray< btTypedConstraint * > m_constraints
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
virtual void updateActivationState(btScalar timeStep)
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
void push_back(const T &_Val)
const btVector3 & getBasePos() const
Definition: btMultiBody.h:166
int getNumLinks() const
Definition: btMultiBody.h:145
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:124
btSimulationIslandManager * m_islandManager
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
const btRigidBody & getRigidBodyA() const
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:113
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size...
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:171
btSimulationIslandManager * getSimulationIslandManager()
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
void addLinkForce(int i, const btVector3 &f)
bool isPosUpdated() const
Definition: btMultiBody.h:530
bool isUsingRK4Integration() const
Definition: btMultiBody.h:526
int getActivationState() const
virtual void integrateTransforms(btScalar timeStep)
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
const btRigidBody & getRigidBodyB() const
virtual void debugDraw(class btIDebugDraw *drawer)=0
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:367
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:849
#define ISLAND_SLEEPING
bool isStaticOrKinematicObject() const
btContactSolverInfo m_solverInfo
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
void clearForcesAndTorques()
int getNumCollisionObjects() const
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
this method should not be called, it was just used during porting/integration of Featherstone btMulti...
virtual int getIslandIdB() const =0
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
btCollisionWorld * getCollisionWorld()
btAlignedObjectArray< btCollisionObject * > m_bodies
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
const btQuaternion & getParentToLocalRot(int i) const
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btIDebugDraw * m_debugDrawer
bool isAwake() const
Definition: btMultiBody.h:446
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
virtual btIDebugDraw * getDebugDrawer()
btScalar getBaseMass() const
Definition: btMultiBody.h:148
const btCollisionObject * getBody0() const
bool isEnabled() const
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
#define output
int getNumDofs() const
Definition: btMultiBody.h:146
btAlignedObjectArray< btPersistentManifold * > m_manifolds
int getParent(int link_num) const
btCollisionObject can be used to manage collision detection objects.
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:28
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs...
void setDeactivationTime(btScalar time)
void setWorldTransform(const btTransform &worldTrans)
btDispatcher * getDispatcher()
void clearVelocities()
virtual void solveConstraints(btContactSolverInfo &solverInfo)
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)
void checkMotionAndSleepIfRequired(btScalar timestep)
btAlignedObjectArray< btMultiBody * > m_multiBodies
virtual int getIslandIdA() const =0
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
int getNumPosVars() const
Definition: btMultiBody.h:147
int size() const
return the number of elements in the array
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:268
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
int getIslandTag() const
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
virtual void prepareSolve(int, int)
#define WANTS_DEACTIVATION
void remove(const T &key)
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:137
virtual void addMultiBody(btMultiBody *body, short group=btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
virtual void integrateTransforms(btScalar timeStep)
const btCollisionObject * getBody1() const
virtual int getDebugMode() const =0
void setPosUpdated(bool updated)
Definition: btMultiBody.h:534
void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
virtual void storeIslandActivationState(btCollisionWorld *world)
#define DISABLE_DEACTIVATION
virtual void removeMultiBody(btMultiBody *body)
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btConstraintSolver * m_constraintSolver
btScalar getLinkMass(int i) const
bool isMultiDof()
Definition: btMultiBody.h:522
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
virtual void updateActivationState(btScalar timeStep)
void stepPositions(btScalar dt)
void unite(int p, int q)
Definition: btUnionFind.h:81
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btContactSolverInfo & getSolverInfo()
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:224
void setActivationState(int newState) const
const btVector3 & getRVector(int i) const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
void quickSort(const L &CompareFunc)
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btAlignedObjectArray< btTypedConstraint * > m_constraints