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
16 #include "btMultiBodyDynamicsWorld.h"
17 #include "btMultiBodyConstraintSolver.h"
18 #include "btMultiBody.h"
19 #include "btMultiBodyLinkCollider.h"
20 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
21 #include "LinearMath/btQuickprof.h"
22 #include "btMultiBodyConstraint.h"
23 #include "LinearMath/btIDebugDraw.h"
24
25
26
addMultiBody(btMultiBody * body,short group,short mask)27 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
28 {
29 m_multiBodies.push_back(body);
30
31 }
32
removeMultiBody(btMultiBody * body)33 void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
34 {
35 m_multiBodies.remove(body);
36 }
37
calculateSimulationIslands()38 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
39 {
40 BT_PROFILE("calculateSimulationIslands");
41
42 getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
43
44 {
45 //merge islands based on speculative contact manifolds too
46 for (int i=0;i<this->m_predictiveManifolds.size();i++)
47 {
48 btPersistentManifold* manifold = m_predictiveManifolds[i];
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 {
86 btMultiBodyLinkCollider* prev = body->getBaseCollider();
87
88 for (int b=0;b<body->getNumLinks();b++)
89 {
90 btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
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 {
110 btMultiBodyConstraint* c = m_multiBodyConstraints[i];
111 int tagA = c->getIslandIdA();
112 int tagB = c->getIslandIdB();
113 if (tagA>=0 && tagB>=0)
114 getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
115 }
116 }
117
118 //Store the island id in each body
119 getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
120
121 }
122
123
updateActivationState(btScalar timeStep)124 void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
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 {
138 btMultiBodyLinkCollider* col = body->getBaseCollider();
139 if (col && col->getActivationState() == ACTIVE_TAG)
140 {
141 col->setActivationState( WANTS_DEACTIVATION);
142 col->setDeactivationTime(0.f);
143 }
144 for (int b=0;b<body->getNumLinks();b++)
145 {
146 btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
147 if (col && col->getActivationState() == ACTIVE_TAG)
148 {
149 col->setActivationState( WANTS_DEACTIVATION);
150 col->setDeactivationTime(0.f);
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 }
169
170 btDiscreteDynamicsWorld::updateActivationState(timeStep);
171 }
172
173
btGetConstraintIslandId2(const btTypedConstraint * lhs)174 SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
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
186 class btSortConstraintOnIslandPredicate2
187 {
188 public:
189
operator ()(const btTypedConstraint * lhs,const btTypedConstraint * rhs) const190 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
btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint * lhs)201 SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
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
213 class btSortMultiBodyConstraintOnIslandPredicate
214 {
215 public:
216
operator ()(const btMultiBodyConstraint * lhs,const btMultiBodyConstraint * rhs) const217 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
226 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
227 {
228 btContactSolverInfo* m_solverInfo;
229 btMultiBodyConstraintSolver* m_solver;
230 btMultiBodyConstraint** m_multiBodySortedConstraints;
231 int m_numMultiBodyConstraints;
232
233 btTypedConstraint** m_sortedConstraints;
234 int m_numConstraints;
235 btIDebugDraw* m_debugDrawer;
236 btDispatcher* m_dispatcher;
237
238 btAlignedObjectArray<btCollisionObject*> m_bodies;
239 btAlignedObjectArray<btPersistentManifold*> m_manifolds;
240 btAlignedObjectArray<btTypedConstraint*> m_constraints;
241 btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
242
243
MultiBodyInplaceSolverIslandCallbackMultiBodyInplaceSolverIslandCallback244 MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver,
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
operator =MultiBodyInplaceSolverIslandCallback256 MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
257 {
258 btAssert(0);
259 (void)other;
260 return *this;
261 }
262
setupMultiBodyInplaceSolverIslandCallback263 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
processIslandMultiBodyInplaceSolverIslandCallback281 virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
282 {
283 if (islandId<0)
284 {
285 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
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 }
processConstraintsMultiBodyInplaceSolverIslandCallback361 void processConstraints()
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
btMultiBodyDynamicsWorld(btDispatcher * dispatcher,btBroadphaseInterface * pairCache,btMultiBodyConstraintSolver * constraintSolver,btCollisionConfiguration * collisionConfiguration)382 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
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;
388 getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
389 m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
390 }
391
~btMultiBodyDynamicsWorld()392 btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
393 {
394 delete m_solverMultiBodyIslandCallback;
395 }
396
solveConstraints(btContactSolverInfo & solverInfo)397 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
398 {
399
400 btAlignedObjectArray<btScalar> scratch_r;
401 btAlignedObjectArray<btVector3> scratch_v;
402 btAlignedObjectArray<btMatrix3x3> scratch_m;
403
404
405 BT_PROFILE("solveConstraints");
406
407 m_sortedConstraints.resize( m_constraints.size());
408 int i;
409 for (i=0;i<getNumConstraints();i++)
410 {
411 m_sortedConstraints[i] = m_constraints[i];
412 }
413 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
414 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
415
416 m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
417 for (i=0;i<m_multiBodyConstraints.size();i++)
418 {
419 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
420 }
421 m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
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());
427 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
428
429 /// solve all the constraints for this island
430 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
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
441 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
442 {
443 isSleeping = true;
444 }
445 for (int b=0;b<bod->getNumLinks();b++)
446 {
447 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
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
493 /////
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];
511 ////
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
638 m_solverMultiBodyIslandCallback->processConstraints();
639
640 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
641
642 }
643
integrateTransforms(btScalar timeStep)644 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
645 {
646 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
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;
658 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
659 {
660 isSleeping = true;
661 }
662 for (int b=0;b<bod->getNumLinks();b++)
663 {
664 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
665 isSleeping = true;
666 }
667
668
669 if (!isSleeping)
670 {
671 int nLinks = bod->getNumLinks();
672
673 ///base + num m_links
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
706 bod->getBaseCollider()->setWorldTransform(tr);
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 {
720 btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
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
addMultiBodyConstraint(btMultiBodyConstraint * constraint)749 void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
750 {
751 m_multiBodyConstraints.push_back(constraint);
752 }
753
removeMultiBodyConstraint(btMultiBodyConstraint * constraint)754 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
755 {
756 m_multiBodyConstraints.remove(constraint);
757 }
758
debugDrawMultiBodyConstraint(btMultiBodyConstraint * constraint)759 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
760 {
761 constraint->debugDraw(getDebugDrawer());
762 }
763
764
debugDrawWorld()765 void btMultiBodyDynamicsWorld::debugDrawWorld()
766 {
767 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
768
769 bool drawConstraints = false;
770 if (getDebugDrawer())
771 {
772 int mode = getDebugDrawer()->getDebugMode();
773 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
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 {
787 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
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
796 ///base + num m_links
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
845 btDiscreteDynamicsWorld::debugDrawWorld();
846 }
847