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 #include "LinearMath/btSerializer.h"
25
addMultiBody(btMultiBody * body,int group,int mask)26 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
27 {
28 m_multiBodies.push_back(body);
29 }
30
removeMultiBody(btMultiBody * body)31 void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
32 {
33 m_multiBodies.remove(body);
34 }
35
predictUnconstraintMotion(btScalar timeStep)36 void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
37 {
38 btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
39 predictMultiBodyTransforms(timeStep);
40
41 }
calculateSimulationIslands()42 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
43 {
44 BT_PROFILE("calculateSimulationIslands");
45
46 getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
47
48 {
49 //merge islands based on speculative contact manifolds too
50 for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
51 {
52 btPersistentManifold* manifold = m_predictiveManifolds[i];
53
54 const btCollisionObject* colObj0 = manifold->getBody0();
55 const btCollisionObject* colObj1 = manifold->getBody1();
56
57 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
58 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
59 {
60 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
61 }
62 }
63 }
64
65 {
66 int i;
67 int numConstraints = int(m_constraints.size());
68 for (i = 0; i < numConstraints; i++)
69 {
70 btTypedConstraint* constraint = m_constraints[i];
71 if (constraint->isEnabled())
72 {
73 const btRigidBody* colObj0 = &constraint->getRigidBodyA();
74 const btRigidBody* colObj1 = &constraint->getRigidBodyB();
75
76 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
77 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
78 {
79 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
80 }
81 }
82 }
83 }
84
85 //merge islands linked by Featherstone link colliders
86 for (int i = 0; i < m_multiBodies.size(); i++)
87 {
88 btMultiBody* body = m_multiBodies[i];
89 {
90 btMultiBodyLinkCollider* prev = body->getBaseCollider();
91
92 for (int b = 0; b < body->getNumLinks(); b++)
93 {
94 btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
95
96 if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
97 ((prev) && (!(prev)->isStaticOrKinematicObject())))
98 {
99 int tagPrev = prev->getIslandTag();
100 int tagCur = cur->getIslandTag();
101 getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
102 }
103 if (cur && !cur->isStaticOrKinematicObject())
104 prev = cur;
105 }
106 }
107 }
108
109 //merge islands linked by multibody constraints
110 {
111 for (int i = 0; i < this->m_multiBodyConstraints.size(); i++)
112 {
113 btMultiBodyConstraint* c = m_multiBodyConstraints[i];
114 int tagA = c->getIslandIdA();
115 int tagB = c->getIslandIdB();
116 if (tagA >= 0 && tagB >= 0)
117 getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
118 }
119 }
120
121 //Store the island id in each body
122 getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
123 }
124
updateActivationState(btScalar timeStep)125 void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
126 {
127 BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
128
129 for (int i = 0; i < m_multiBodies.size(); i++)
130 {
131 btMultiBody* body = m_multiBodies[i];
132 if (body)
133 {
134 body->checkMotionAndSleepIfRequired(timeStep);
135 if (!body->isAwake())
136 {
137 btMultiBodyLinkCollider* col = body->getBaseCollider();
138 if (col && col->getActivationState() == ACTIVE_TAG)
139 {
140 if (body->hasFixedBase())
141 {
142 col->setActivationState(FIXED_BASE_MULTI_BODY);
143 } else
144 {
145 col->setActivationState(WANTS_DEACTIVATION);
146 }
147
148 col->setDeactivationTime(0.f);
149 }
150 for (int b = 0; b < body->getNumLinks(); b++)
151 {
152 btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
153 if (col && col->getActivationState() == ACTIVE_TAG)
154 {
155 col->setActivationState(WANTS_DEACTIVATION);
156 col->setDeactivationTime(0.f);
157 }
158 }
159 }
160 else
161 {
162 btMultiBodyLinkCollider* col = body->getBaseCollider();
163 if (col && col->getActivationState() != DISABLE_DEACTIVATION)
164 col->setActivationState(ACTIVE_TAG);
165
166 for (int b = 0; b < body->getNumLinks(); b++)
167 {
168 btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
169 if (col && col->getActivationState() != DISABLE_DEACTIVATION)
170 col->setActivationState(ACTIVE_TAG);
171 }
172 }
173 }
174 }
175
176 btDiscreteDynamicsWorld::updateActivationState(timeStep);
177 }
178
getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData> & islandAnalyticsData) const179 void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
180 {
181 islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
182 }
183
btMultiBodyDynamicsWorld(btDispatcher * dispatcher,btBroadphaseInterface * pairCache,btMultiBodyConstraintSolver * constraintSolver,btCollisionConfiguration * collisionConfiguration)184 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
185 : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
186 m_multiBodyConstraintSolver(constraintSolver)
187 {
188 //split impulse is not yet supported for Featherstone hierarchies
189 // getSolverInfo().m_splitImpulse = false;
190 getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
191 m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
192 }
193
~btMultiBodyDynamicsWorld()194 btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld()
195 {
196 delete m_solverMultiBodyIslandCallback;
197 }
198
setMultiBodyConstraintSolver(btMultiBodyConstraintSolver * solver)199 void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
200 {
201 m_multiBodyConstraintSolver = solver;
202 m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
203 btDiscreteDynamicsWorld::setConstraintSolver(solver);
204 }
205
setConstraintSolver(btConstraintSolver * solver)206 void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
207 {
208 if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
209 {
210 m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
211 }
212 btDiscreteDynamicsWorld::setConstraintSolver(solver);
213 }
214
forwardKinematics()215 void btMultiBodyDynamicsWorld::forwardKinematics()
216 {
217 for (int b = 0; b < m_multiBodies.size(); b++)
218 {
219 btMultiBody* bod = m_multiBodies[b];
220 bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin);
221 }
222 }
solveConstraints(btContactSolverInfo & solverInfo)223 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
224 {
225 solveExternalForces(solverInfo);
226 buildIslands();
227 solveInternalConstraints(solverInfo);
228 }
229
buildIslands()230 void btMultiBodyDynamicsWorld::buildIslands()
231 {
232 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
233 }
234
solveInternalConstraints(btContactSolverInfo & solverInfo)235 void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
236 {
237 /// solve all the constraints for this island
238 m_solverMultiBodyIslandCallback->processConstraints();
239 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
240 {
241 BT_PROFILE("btMultiBody stepVelocities");
242 for (int i = 0; i < this->m_multiBodies.size(); i++)
243 {
244 btMultiBody* bod = m_multiBodies[i];
245
246 bool isSleeping = false;
247
248 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
249 {
250 isSleeping = true;
251 }
252 for (int b = 0; b < bod->getNumLinks(); b++)
253 {
254 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
255 isSleeping = true;
256 }
257
258 if (!isSleeping)
259 {
260 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
261 m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
262 m_scratch_v.resize(bod->getNumLinks() + 1);
263 m_scratch_m.resize(bod->getNumLinks() + 1);
264
265 if (bod->internalNeedsJointFeedback())
266 {
267 if (!bod->isUsingRK4Integration())
268 {
269 if (bod->internalNeedsJointFeedback())
270 {
271 bool isConstraintPass = true;
272 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
273 getSolverInfo().m_jointFeedbackInWorldSpace,
274 getSolverInfo().m_jointFeedbackInJointFrame);
275 }
276 }
277 }
278 }
279 }
280 }
281 for (int i = 0; i < this->m_multiBodies.size(); i++)
282 {
283 btMultiBody* bod = m_multiBodies[i];
284 bod->processDeltaVeeMultiDof2();
285 }
286 }
287
solveExternalForces(btContactSolverInfo & solverInfo)288 void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
289 {
290 forwardKinematics();
291
292 BT_PROFILE("solveConstraints");
293
294 clearMultiBodyConstraintForces();
295
296 m_sortedConstraints.resize(m_constraints.size());
297 int i;
298 for (i = 0; i < getNumConstraints(); i++)
299 {
300 m_sortedConstraints[i] = m_constraints[i];
301 }
302 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
303 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
304
305 m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
306 for (i = 0; i < m_multiBodyConstraints.size(); i++)
307 {
308 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
309 }
310 m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
311
312 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
313
314 m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
315 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
316
317 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
318 {
319 BT_PROFILE("btMultiBody addForce");
320 for (int i = 0; i < this->m_multiBodies.size(); i++)
321 {
322 btMultiBody* bod = m_multiBodies[i];
323
324 bool isSleeping = false;
325
326 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
327 {
328 isSleeping = true;
329 }
330 for (int b = 0; b < bod->getNumLinks(); b++)
331 {
332 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
333 isSleeping = true;
334 }
335
336 if (!isSleeping)
337 {
338 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
339 m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
340 m_scratch_v.resize(bod->getNumLinks() + 1);
341 m_scratch_m.resize(bod->getNumLinks() + 1);
342
343 bod->addBaseForce(m_gravity * bod->getBaseMass());
344
345 for (int j = 0; j < bod->getNumLinks(); ++j)
346 {
347 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
348 }
349 } //if (!isSleeping)
350 }
351 }
352 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
353
354 {
355 BT_PROFILE("btMultiBody stepVelocities");
356 for (int i = 0; i < this->m_multiBodies.size(); i++)
357 {
358 btMultiBody* bod = m_multiBodies[i];
359
360 bool isSleeping = false;
361
362 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
363 {
364 isSleeping = true;
365 }
366 for (int b = 0; b < bod->getNumLinks(); b++)
367 {
368 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
369 isSleeping = true;
370 }
371
372 if (!isSleeping)
373 {
374 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
375 m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
376 m_scratch_v.resize(bod->getNumLinks() + 1);
377 m_scratch_m.resize(bod->getNumLinks() + 1);
378 bool doNotUpdatePos = false;
379 bool isConstraintPass = false;
380 {
381 if (!bod->isUsingRK4Integration())
382 {
383 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
384 m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
385 getSolverInfo().m_jointFeedbackInWorldSpace,
386 getSolverInfo().m_jointFeedbackInJointFrame);
387 }
388 else
389 {
390 //
391 int numDofs = bod->getNumDofs() + 6;
392 int numPosVars = bod->getNumPosVars() + 7;
393 btAlignedObjectArray<btScalar> scratch_r2;
394 scratch_r2.resize(2 * numPosVars + 8 * numDofs);
395 //convenience
396 btScalar* pMem = &scratch_r2[0];
397 btScalar* scratch_q0 = pMem;
398 pMem += numPosVars;
399 btScalar* scratch_qx = pMem;
400 pMem += numPosVars;
401 btScalar* scratch_qd0 = pMem;
402 pMem += numDofs;
403 btScalar* scratch_qd1 = pMem;
404 pMem += numDofs;
405 btScalar* scratch_qd2 = pMem;
406 pMem += numDofs;
407 btScalar* scratch_qd3 = pMem;
408 pMem += numDofs;
409 btScalar* scratch_qdd0 = pMem;
410 pMem += numDofs;
411 btScalar* scratch_qdd1 = pMem;
412 pMem += numDofs;
413 btScalar* scratch_qdd2 = pMem;
414 pMem += numDofs;
415 btScalar* scratch_qdd3 = pMem;
416 pMem += numDofs;
417 btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
418
419 /////
420 //copy q0 to scratch_q0 and qd0 to scratch_qd0
421 scratch_q0[0] = bod->getWorldToBaseRot().x();
422 scratch_q0[1] = bod->getWorldToBaseRot().y();
423 scratch_q0[2] = bod->getWorldToBaseRot().z();
424 scratch_q0[3] = bod->getWorldToBaseRot().w();
425 scratch_q0[4] = bod->getBasePos().x();
426 scratch_q0[5] = bod->getBasePos().y();
427 scratch_q0[6] = bod->getBasePos().z();
428 //
429 for (int link = 0; link < bod->getNumLinks(); ++link)
430 {
431 for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
432 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
433 }
434 //
435 for (int dof = 0; dof < numDofs; ++dof)
436 scratch_qd0[dof] = bod->getVelocityVector()[dof];
437 ////
438 struct
439 {
440 btMultiBody* bod;
441 btScalar *scratch_qx, *scratch_q0;
442
443 void operator()()
444 {
445 for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
446 scratch_qx[dof] = scratch_q0[dof];
447 }
448 } pResetQx = {bod, scratch_qx, scratch_q0};
449 //
450 struct
451 {
452 void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
453 {
454 for (int i = 0; i < size; ++i)
455 pVal[i] = pCurVal[i] + dt * pDer[i];
456 }
457
458 } pEulerIntegrate;
459 //
460 struct
461 {
462 void operator()(btMultiBody* pBody, const btScalar* pData)
463 {
464 btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
465
466 for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
467 pVel[i] = pData[i];
468 }
469 } pCopyToVelocityVector;
470 //
471 struct
472 {
473 void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
474 {
475 for (int i = 0; i < size; ++i)
476 pDst[i] = pSrc[start + i];
477 }
478 } pCopy;
479 //
480
481 btScalar h = solverInfo.m_timeStep;
482 #define output &m_scratch_r[bod->getNumDofs()]
483 //calc qdd0 from: q0 & qd0
484 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
485 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
486 getSolverInfo().m_jointFeedbackInJointFrame);
487 pCopy(output, scratch_qdd0, 0, numDofs);
488 //calc q1 = q0 + h/2 * qd0
489 pResetQx();
490 bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
491 //calc qd1 = qd0 + h/2 * qdd0
492 pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
493 //
494 //calc qdd1 from: q1 & qd1
495 pCopyToVelocityVector(bod, scratch_qd1);
496 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
497 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
498 getSolverInfo().m_jointFeedbackInJointFrame);
499 pCopy(output, scratch_qdd1, 0, numDofs);
500 //calc q2 = q0 + h/2 * qd1
501 pResetQx();
502 bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
503 //calc qd2 = qd0 + h/2 * qdd1
504 pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
505 //
506 //calc qdd2 from: q2 & qd2
507 pCopyToVelocityVector(bod, scratch_qd2);
508 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
509 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
510 getSolverInfo().m_jointFeedbackInJointFrame);
511 pCopy(output, scratch_qdd2, 0, numDofs);
512 //calc q3 = q0 + h * qd2
513 pResetQx();
514 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
515 //calc qd3 = qd0 + h * qdd2
516 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
517 //
518 //calc qdd3 from: q3 & qd3
519 pCopyToVelocityVector(bod, scratch_qd3);
520 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
521 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
522 getSolverInfo().m_jointFeedbackInJointFrame);
523 pCopy(output, scratch_qdd3, 0, numDofs);
524
525 //
526 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
527 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
528 btAlignedObjectArray<btScalar> delta_q;
529 delta_q.resize(numDofs);
530 btAlignedObjectArray<btScalar> delta_qd;
531 delta_qd.resize(numDofs);
532 for (int i = 0; i < numDofs; ++i)
533 {
534 delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
535 delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
536 //delta_q[i] = h*scratch_qd0[i];
537 //delta_qd[i] = h*scratch_qdd0[i];
538 }
539 //
540 pCopyToVelocityVector(bod, scratch_qd0);
541 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
542 //
543 if (!doNotUpdatePos)
544 {
545 btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
546 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
547
548 for (int i = 0; i < numDofs; ++i)
549 pRealBuf[i] = delta_q[i];
550
551 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
552 bod->setPosUpdated(true);
553 }
554
555 //ugly hack which resets the cached data to t0 (needed for constraint solver)
556 {
557 for (int link = 0; link < bod->getNumLinks(); ++link)
558 bod->getLink(link).updateCacheMultiDof();
559 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
560 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
561 getSolverInfo().m_jointFeedbackInJointFrame);
562 }
563 }
564 }
565
566 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
567 bod->clearForcesAndTorques();
568 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
569 } //if (!isSleeping)
570 }
571 }
572 }
573
574
integrateTransforms(btScalar timeStep)575 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
576 {
577 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
578 integrateMultiBodyTransforms(timeStep);
579 }
580
integrateMultiBodyTransforms(btScalar timeStep)581 void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
582 {
583 BT_PROFILE("btMultiBody stepPositions");
584 //integrate and update the Featherstone hierarchies
585
586 for (int b = 0; b < m_multiBodies.size(); b++)
587 {
588 btMultiBody* bod = m_multiBodies[b];
589 bool isSleeping = false;
590 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
591 {
592 isSleeping = true;
593 }
594 for (int b = 0; b < bod->getNumLinks(); b++)
595 {
596 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
597 isSleeping = true;
598 }
599
600 if (!isSleeping)
601 {
602 bod->addSplitV();
603 int nLinks = bod->getNumLinks();
604
605 ///base + num m_links
606 if (!bod->isPosUpdated())
607 bod->stepPositionsMultiDof(timeStep);
608 else
609 {
610 btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
611 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
612
613 bod->stepPositionsMultiDof(1, 0, pRealBuf);
614 bod->setPosUpdated(false);
615 }
616
617
618 m_scratch_world_to_local.resize(nLinks + 1);
619 m_scratch_local_origin.resize(nLinks + 1);
620 bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
621 bod->substractSplitV();
622 }
623 else
624 {
625 bod->clearVelocities();
626 }
627 }
628 }
629
predictMultiBodyTransforms(btScalar timeStep)630 void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
631 {
632 BT_PROFILE("btMultiBody stepPositions");
633 //integrate and update the Featherstone hierarchies
634
635 for (int b = 0; b < m_multiBodies.size(); b++)
636 {
637 btMultiBody* bod = m_multiBodies[b];
638 bool isSleeping = false;
639 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
640 {
641 isSleeping = true;
642 }
643 for (int b = 0; b < bod->getNumLinks(); b++)
644 {
645 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
646 isSleeping = true;
647 }
648
649 if (!isSleeping)
650 {
651 int nLinks = bod->getNumLinks();
652 bod->predictPositionsMultiDof(timeStep);
653 m_scratch_world_to_local.resize(nLinks + 1);
654 m_scratch_local_origin.resize(nLinks + 1);
655 bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
656 }
657 else
658 {
659 bod->clearVelocities();
660 }
661 }
662 }
663
addMultiBodyConstraint(btMultiBodyConstraint * constraint)664 void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
665 {
666 m_multiBodyConstraints.push_back(constraint);
667 }
668
removeMultiBodyConstraint(btMultiBodyConstraint * constraint)669 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint)
670 {
671 m_multiBodyConstraints.remove(constraint);
672 }
673
debugDrawMultiBodyConstraint(btMultiBodyConstraint * constraint)674 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
675 {
676 constraint->debugDraw(getDebugDrawer());
677 }
678
debugDrawWorld()679 void btMultiBodyDynamicsWorld::debugDrawWorld()
680 {
681 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
682
683 btDiscreteDynamicsWorld::debugDrawWorld();
684
685 bool drawConstraints = false;
686 if (getDebugDrawer())
687 {
688 int mode = getDebugDrawer()->getDebugMode();
689 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
690 {
691 drawConstraints = true;
692 }
693
694 if (drawConstraints)
695 {
696 BT_PROFILE("btMultiBody debugDrawWorld");
697
698 for (int c = 0; c < m_multiBodyConstraints.size(); c++)
699 {
700 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
701 debugDrawMultiBodyConstraint(constraint);
702 }
703
704 for (int b = 0; b < m_multiBodies.size(); b++)
705 {
706 btMultiBody* bod = m_multiBodies[b];
707 bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1);
708
709 if (mode & btIDebugDraw::DBG_DrawFrames)
710 {
711 getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
712 }
713
714 for (int m = 0; m < bod->getNumLinks(); m++)
715 {
716 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
717 if (mode & btIDebugDraw::DBG_DrawFrames)
718 {
719 getDebugDrawer()->drawTransform(tr, 0.1);
720 }
721 //draw the joint axis
722 if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute)
723 {
724 btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
725
726 btVector4 color(0, 0, 0, 1); //1,1,1);
727 btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
728 btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
729 getDebugDrawer()->drawLine(from, to, color);
730 }
731 if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed)
732 {
733 btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
734
735 btVector4 color(0, 0, 0, 1); //1,1,1);
736 btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
737 btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
738 getDebugDrawer()->drawLine(from, to, color);
739 }
740 if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic)
741 {
742 btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
743
744 btVector4 color(0, 0, 0, 1); //1,1,1);
745 btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
746 btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
747 getDebugDrawer()->drawLine(from, to, color);
748 }
749 }
750 }
751 }
752 }
753 }
754
applyGravity()755 void btMultiBodyDynamicsWorld::applyGravity()
756 {
757 btDiscreteDynamicsWorld::applyGravity();
758 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
759 BT_PROFILE("btMultiBody addGravity");
760 for (int i = 0; i < this->m_multiBodies.size(); i++)
761 {
762 btMultiBody* bod = m_multiBodies[i];
763
764 bool isSleeping = false;
765
766 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
767 {
768 isSleeping = true;
769 }
770 for (int b = 0; b < bod->getNumLinks(); b++)
771 {
772 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
773 isSleeping = true;
774 }
775
776 if (!isSleeping)
777 {
778 bod->addBaseForce(m_gravity * bod->getBaseMass());
779
780 for (int j = 0; j < bod->getNumLinks(); ++j)
781 {
782 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
783 }
784 } //if (!isSleeping)
785 }
786 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
787 }
788
clearMultiBodyConstraintForces()789 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
790 {
791 for (int i = 0; i < this->m_multiBodies.size(); i++)
792 {
793 btMultiBody* bod = m_multiBodies[i];
794 bod->clearConstraintForces();
795 }
796 }
clearMultiBodyForces()797 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
798 {
799 {
800 // BT_PROFILE("clearMultiBodyForces");
801 for (int i = 0; i < this->m_multiBodies.size(); i++)
802 {
803 btMultiBody* bod = m_multiBodies[i];
804
805 bool isSleeping = false;
806
807 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
808 {
809 isSleeping = true;
810 }
811 for (int b = 0; b < bod->getNumLinks(); b++)
812 {
813 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
814 isSleeping = true;
815 }
816
817 if (!isSleeping)
818 {
819 btMultiBody* bod = m_multiBodies[i];
820 bod->clearForcesAndTorques();
821 }
822 }
823 }
824 }
clearForces()825 void btMultiBodyDynamicsWorld::clearForces()
826 {
827 btDiscreteDynamicsWorld::clearForces();
828
829 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
830 clearMultiBodyForces();
831 #endif
832 }
833
serialize(btSerializer * serializer)834 void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
835 {
836 serializer->startSerialization();
837
838 serializeDynamicsWorldInfo(serializer);
839
840 serializeMultiBodies(serializer);
841
842 serializeRigidBodies(serializer);
843
844 serializeCollisionObjects(serializer);
845
846 serializeContactManifolds(serializer);
847
848 serializer->finishSerialization();
849 }
850
serializeMultiBodies(btSerializer * serializer)851 void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
852 {
853 int i;
854 //serialize all collision objects
855 for (i = 0; i < m_multiBodies.size(); i++)
856 {
857 btMultiBody* mb = m_multiBodies[i];
858 {
859 int len = mb->calculateSerializeBufferSize();
860 btChunk* chunk = serializer->allocate(len, 1);
861 const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
862 serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
863 }
864 }
865
866 //serialize all multibody links (collision objects)
867 for (i = 0; i < m_collisionObjects.size(); i++)
868 {
869 btCollisionObject* colObj = m_collisionObjects[i];
870 if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
871 {
872 int len = colObj->calculateSerializeBufferSize();
873 btChunk* chunk = serializer->allocate(len, 1);
874 const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
875 serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
876 }
877 }
878 }
879
saveKinematicState(btScalar timeStep)880 void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep)
881 {
882 btDiscreteDynamicsWorld::saveKinematicState(timeStep);
883 for(int i = 0; i < m_multiBodies.size(); i++)
884 {
885 btMultiBody* body = m_multiBodies[i];
886 if(body->isBaseKinematic())
887 body->saveKinematicState(timeStep);
888 }
889 }
890
891 //
892 //void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
893 //{
894 // m_islandManager->setSplitIslands(split);
895 //}
896