Bullet Collision Detection & Physics Library
btMultiBodyDynamicsWorld.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
18#include "btMultiBody.h"
25
26
28{
29 m_multiBodies.push_back(body);
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),
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);
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 {
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 {
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 {
312 {
313 numCurConstraints++;
314 }
315 }
316
317 for (i=0;i<m_numMultiBodyConstraints;i++)
318 {
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 {
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
352 {
354 } else
355 {
356 //printf("deferred\n");
357 }
358 }
359 }
360 }
362 {
363
364 btCollisionObject** bodies = m_bodies.size()? &m_bodies[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);
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
400 for (int b=0;b<m_multiBodies.size();b++)
401 {
402 btMultiBody* bod = m_multiBodies[b];
404 }
405}
407{
409
410
411
412 BT_PROFILE("solveConstraints");
413
414 m_sortedConstraints.resize( m_constraints.size());
415 int i;
416 for (i=0;i<getNumConstraints();i++)
417 {
419 }
421 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
422
424 for (i=0;i<m_multiBodyConstraints.size();i++)
425 {
427 }
429
430 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
431
432
433 m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
435
438
439#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
440 {
441 BT_PROFILE("btMultiBody addForce");
442 for (int i=0;i<this->m_multiBodies.size();i++)
443 {
444 btMultiBody* bod = m_multiBodies[i];
445
446 bool isSleeping = false;
447
449 {
450 isSleeping = true;
451 }
452 for (int b=0;b<bod->getNumLinks();b++)
453 {
455 isSleeping = true;
456 }
457
458 if (!isSleeping)
459 {
460 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
461 m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
463 m_scratch_m.resize(bod->getNumLinks()+1);
464
465 bod->addBaseForce(m_gravity * bod->getBaseMass());
466
467 for (int j = 0; j < bod->getNumLinks(); ++j)
468 {
469 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
470 }
471 }//if (!isSleeping)
472 }
473 }
474#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
475
476
477 {
478 BT_PROFILE("btMultiBody stepVelocities");
479 for (int i=0;i<this->m_multiBodies.size();i++)
480 {
481 btMultiBody* bod = m_multiBodies[i];
482
483 bool isSleeping = false;
484
486 {
487 isSleeping = true;
488 }
489 for (int b=0;b<bod->getNumLinks();b++)
490 {
492 isSleeping = true;
493 }
494
495 if (!isSleeping)
496 {
497 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
498 m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
500 m_scratch_m.resize(bod->getNumLinks()+1);
501 bool doNotUpdatePos = false;
502
503 {
504 if(!bod->isUsingRK4Integration())
505 {
507 }
508 else
509 {
510 //
511 int numDofs = bod->getNumDofs() + 6;
512 int numPosVars = bod->getNumPosVars() + 7;
513 btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
514 //convenience
515 btScalar *pMem = &scratch_r2[0];
516 btScalar *scratch_q0 = pMem; pMem += numPosVars;
517 btScalar *scratch_qx = pMem; pMem += numPosVars;
518 btScalar *scratch_qd0 = pMem; pMem += numDofs;
519 btScalar *scratch_qd1 = pMem; pMem += numDofs;
520 btScalar *scratch_qd2 = pMem; pMem += numDofs;
521 btScalar *scratch_qd3 = pMem; pMem += numDofs;
522 btScalar *scratch_qdd0 = pMem; pMem += numDofs;
523 btScalar *scratch_qdd1 = pMem; pMem += numDofs;
524 btScalar *scratch_qdd2 = pMem; pMem += numDofs;
525 btScalar *scratch_qdd3 = pMem; pMem += numDofs;
526 btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
527
529 //copy q0 to scratch_q0 and qd0 to scratch_qd0
530 scratch_q0[0] = bod->getWorldToBaseRot().x();
531 scratch_q0[1] = bod->getWorldToBaseRot().y();
532 scratch_q0[2] = bod->getWorldToBaseRot().z();
533 scratch_q0[3] = bod->getWorldToBaseRot().w();
534 scratch_q0[4] = bod->getBasePos().x();
535 scratch_q0[5] = bod->getBasePos().y();
536 scratch_q0[6] = bod->getBasePos().z();
537 //
538 for(int link = 0; link < bod->getNumLinks(); ++link)
539 {
540 for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
541 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
542 }
543 //
544 for(int dof = 0; dof < numDofs; ++dof)
545 scratch_qd0[dof] = bod->getVelocityVector()[dof];
547 struct
548 {
549 btMultiBody *bod;
550 btScalar *scratch_qx, *scratch_q0;
551
552 void operator()()
553 {
554 for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
555 scratch_qx[dof] = scratch_q0[dof];
556 }
557 } pResetQx = {bod, scratch_qx, scratch_q0};
558 //
559 struct
560 {
561 void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
562 {
563 for(int i = 0; i < size; ++i)
564 pVal[i] = pCurVal[i] + dt * pDer[i];
565 }
566
567 } pEulerIntegrate;
568 //
569 struct
570 {
571 void operator()(btMultiBody *pBody, const btScalar *pData)
572 {
573 btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
574
575 for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
576 pVel[i] = pData[i];
577
578 }
579 } pCopyToVelocityVector;
580 //
581 struct
582 {
583 void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
584 {
585 for(int i = 0; i < size; ++i)
586 pDst[i] = pSrc[start + i];
587 }
588 } pCopy;
589 //
590
591 btScalar h = solverInfo.m_timeStep;
592 #define output &m_scratch_r[bod->getNumDofs()]
593 //calc qdd0 from: q0 & qd0
595 pCopy(output, scratch_qdd0, 0, numDofs);
596 //calc q1 = q0 + h/2 * qd0
597 pResetQx();
598 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
599 //calc qd1 = qd0 + h/2 * qdd0
600 pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
601 //
602 //calc qdd1 from: q1 & qd1
603 pCopyToVelocityVector(bod, scratch_qd1);
605 pCopy(output, scratch_qdd1, 0, numDofs);
606 //calc q2 = q0 + h/2 * qd1
607 pResetQx();
608 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
609 //calc qd2 = qd0 + h/2 * qdd1
610 pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
611 //
612 //calc qdd2 from: q2 & qd2
613 pCopyToVelocityVector(bod, scratch_qd2);
615 pCopy(output, scratch_qdd2, 0, numDofs);
616 //calc q3 = q0 + h * qd2
617 pResetQx();
618 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
619 //calc qd3 = qd0 + h * qdd2
620 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
621 //
622 //calc qdd3 from: q3 & qd3
623 pCopyToVelocityVector(bod, scratch_qd3);
625 pCopy(output, scratch_qdd3, 0, numDofs);
626
627 //
628 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
629 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
630 btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
631 btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
632 for(int i = 0; i < numDofs; ++i)
633 {
634 delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
635 delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
636 //delta_q[i] = h*scratch_qd0[i];
637 //delta_qd[i] = h*scratch_qdd0[i];
638 }
639 //
640 pCopyToVelocityVector(bod, scratch_qd0);
641 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
642 //
643 if(!doNotUpdatePos)
644 {
645 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
646 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
647
648 for(int i = 0; i < numDofs; ++i)
649 pRealBuf[i] = delta_q[i];
650
651 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
652 bod->setPosUpdated(true);
653 }
654
655 //ugly hack which resets the cached data to t0 (needed for constraint solver)
656 {
657 for(int link = 0; link < bod->getNumLinks(); ++link)
658 bod->getLink(link).updateCacheMultiDof();
660 }
661
662 }
663 }
664
665#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
667#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
668 }//if (!isSleeping)
669 }
670 }
671
673
675
677
678 {
679 BT_PROFILE("btMultiBody stepVelocities");
680 for (int i=0;i<this->m_multiBodies.size();i++)
681 {
682 btMultiBody* bod = m_multiBodies[i];
683
684 bool isSleeping = false;
685
687 {
688 isSleeping = true;
689 }
690 for (int b=0;b<bod->getNumLinks();b++)
691 {
693 isSleeping = true;
694 }
695
696 if (!isSleeping)
697 {
698 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
699 m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
701 m_scratch_m.resize(bod->getNumLinks()+1);
702
703
704 {
705 if(!bod->isUsingRK4Integration())
706 {
707 bool isConstraintPass = true;
709 }
710 }
711 }
712 }
713 }
714
715 for (int i=0;i<this->m_multiBodies.size();i++)
716 {
717 btMultiBody* bod = m_multiBodies[i];
719 }
720
721}
722
724{
726
727 {
728 BT_PROFILE("btMultiBody stepPositions");
729 //integrate and update the Featherstone hierarchies
730
731 for (int b=0;b<m_multiBodies.size();b++)
732 {
733 btMultiBody* bod = m_multiBodies[b];
734 bool isSleeping = false;
736 {
737 isSleeping = true;
738 }
739 for (int b=0;b<bod->getNumLinks();b++)
740 {
742 isSleeping = true;
743 }
744
745
746 if (!isSleeping)
747 {
748 int nLinks = bod->getNumLinks();
749
751
752
753 {
754 if(!bod->isPosUpdated())
755 bod->stepPositionsMultiDof(timeStep);
756 else
757 {
758 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
759 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
760
761 bod->stepPositionsMultiDof(1, 0, pRealBuf);
762 bod->setPosUpdated(false);
763 }
764 }
765
766 m_scratch_world_to_local.resize(nLinks+1);
768
770
771 } else
772 {
773 bod->clearVelocities();
774 }
775 }
776 }
777}
778
779
780
782{
783 m_multiBodyConstraints.push_back(constraint);
784}
785
787{
788 m_multiBodyConstraints.remove(constraint);
789}
790
792{
793 constraint->debugDraw(getDebugDrawer());
794}
795
796
798{
799 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
800
802
803 bool drawConstraints = false;
804 if (getDebugDrawer())
805 {
806 int mode = getDebugDrawer()->getDebugMode();
808 {
809 drawConstraints = true;
810 }
811
812 if (drawConstraints)
813 {
814 BT_PROFILE("btMultiBody debugDrawWorld");
815
816
817 for (int c=0;c<m_multiBodyConstraints.size();c++)
818 {
821 }
822
823 for (int b = 0; b<m_multiBodies.size(); b++)
824 {
825 btMultiBody* bod = m_multiBodies[b];
827
829
830
831 for (int m = 0; m<bod->getNumLinks(); m++)
832 {
833
834 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
835
836 getDebugDrawer()->drawTransform(tr, 0.1);
837
838 //draw the joint axis
840 {
841 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
842
843 btVector4 color(0,0,0,1);//1,1,1);
844 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
846 getDebugDrawer()->drawLine(from,to,color);
847 }
849 {
851
852 btVector4 color(0,0,0,1);//1,1,1);
853 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
855 getDebugDrawer()->drawLine(from,to,color);
856 }
858 {
860
861 btVector4 color(0,0,0,1);//1,1,1);
862 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
864 getDebugDrawer()->drawLine(from,to,color);
865 }
866
867 }
868 }
869 }
870 }
871
872
873}
874
875
876
878{
880#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
881 BT_PROFILE("btMultiBody addGravity");
882 for (int i=0;i<this->m_multiBodies.size();i++)
883 {
884 btMultiBody* bod = m_multiBodies[i];
885
886 bool isSleeping = false;
887
889 {
890 isSleeping = true;
891 }
892 for (int b=0;b<bod->getNumLinks();b++)
893 {
895 isSleeping = true;
896 }
897
898 if (!isSleeping)
899 {
900 bod->addBaseForce(m_gravity * bod->getBaseMass());
901
902 for (int j = 0; j < bod->getNumLinks(); ++j)
903 {
904 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
905 }
906 }//if (!isSleeping)
907 }
908#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
909}
910
912{
913 for (int i=0;i<this->m_multiBodies.size();i++)
914 {
915 btMultiBody* bod = m_multiBodies[i];
917 }
918}
920{
921 {
922 // BT_PROFILE("clearMultiBodyForces");
923 for (int i=0;i<this->m_multiBodies.size();i++)
924 {
925 btMultiBody* bod = m_multiBodies[i];
926
927 bool isSleeping = false;
928
930 {
931 isSleeping = true;
932 }
933 for (int b=0;b<bod->getNumLinks();b++)
934 {
936 isSleeping = true;
937 }
938
939 if (!isSleeping)
940 {
941 btMultiBody* bod = m_multiBodies[i];
943 }
944 }
945 }
946
947}
949{
951
952#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
954#endif
955}
956
957
958
959
961{
962
963 serializer->startSerialization();
964
965 serializeDynamicsWorldInfo( serializer);
966
967 serializeMultiBodies(serializer);
968
969 serializeRigidBodies(serializer);
970
971 serializeCollisionObjects(serializer);
972
973 serializer->finishSerialization();
974}
975
977{
978 int i;
979 //serialize all collision objects
980 for (i=0;i<m_multiBodies.size();i++)
981 {
982 btMultiBody* mb = m_multiBodies[i];
983 {
984 int len = mb->calculateSerializeBufferSize();
985 btChunk* chunk = serializer->allocate(len,1);
986 const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
987 serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
988 }
989 }
990
991}
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
@ SOLVER_USE_2_FRICTION_DIRECTIONS
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
#define output
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
#define btAssert(x)
Definition: btScalar.h:131
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:117
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
void * m_oldPtr
Definition: btSerializer.h:56
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size,...
btCollisionObject can be used to manage collision detection objects.
bool isStaticOrKinematicObject() const
void setActivationState(int newState) const
int getIslandTag() const
void setDeactivationTime(btScalar time)
int getActivationState() const
btDispatcher * getDispatcher()
virtual btIDebugDraw * getDebugDrawer()
int getNumCollisionObjects() const
btIDebugDraw * m_debugDrawer
void serializeCollisionObjects(btSerializer *serializer)
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
virtual void integrateTransforms(btScalar timeStep)
void serializeRigidBodies(btSerializer *serializer)
void serializeDynamicsWorldInfo(btSerializer *serializer)
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
virtual void applyGravity()
apply gravity, call this once per timestep
btSimulationIslandManager * m_islandManager
btAlignedObjectArray< btTypedConstraint * > m_constraints
btSimulationIslandManager * getSimulationIslandManager()
virtual void updateActivationState(btScalar timeStep)
btConstraintSolver * m_constraintSolver
btCollisionWorld * getCollisionWorld()
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
btContactSolverInfo & getSolverInfo()
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:30
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
virtual int getDebugMode() const =0
@ DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:71
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)
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 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 removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
virtual void integrateTransforms(btScalar timeStep)
btAlignedObjectArray< btMultiBody * > m_multiBodies
virtual void solveConstraints(btContactSolverInfo &solverInfo)
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
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 applyGravity()
apply gravity, call this once per timestep
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
bool isAwake() const
Definition: btMultiBody.h:472
const btVector3 & getBasePos() const
Definition: btMultiBody.h:186
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
void clearVelocities()
int getNumLinks() const
Definition: btMultiBody.h:164
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
void checkMotionAndSleepIfRequired(btScalar timestep)
btScalar getLinkMass(int i) const
int getNumDofs() const
Definition: btMultiBody.h:165
void addLinkForce(int i, const btVector3 &f)
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:390
void clearConstraintForces()
void setPosUpdated(bool updated)
Definition: btMultiBody.h:560
bool isUsingRK4Integration() const
Definition: btMultiBody.h:552
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:209
btScalar getBaseMass() const
Definition: btMultiBody.h:167
int getNumPosVars() const
Definition: btMultiBody.h:166
virtual int calculateSerializeBufferSize() const
bool isPosUpdated() const
Definition: btMultiBody.h:556
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:191
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:308
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:258
void clearForcesAndTorques()
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * getBody0() const
const btCollisionObject * getBody1() const
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
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)
bool operator()(const btTypedConstraint *lhs, const btTypedConstraint *rhs) const
bool operator()(const btMultiBodyConstraint *lhs, const btMultiBodyConstraint *rhs) const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
TypedConstraint is the baseclass for Bullet constraints and vehicles.
bool isEnabled() const
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
void unite(int p, int q)
Definition: btUnionFind.h:81
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
MultiBodyInplaceSolverIslandCallback & operator=(MultiBodyInplaceSolverIslandCallback &other)
btAlignedObjectArray< btTypedConstraint * > m_constraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
btAlignedObjectArray< btPersistentManifold * > m_manifolds
void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
btAlignedObjectArray< btCollisionObject * > m_bodies
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)