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