Bullet Collision Detection & Physics Library: btMultiBodyDynamicsWorld.cpp Source File

Bullet Collision Detection & Physics Library

btMultiBodyDynamicsWorld.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
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
26 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
27 {
28 m_multiBodies.push_back(body);
29 }
30
31 void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
32 {
33 m_multiBodies.remove(body);
34 }
35
36 void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
37 {
38 btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
39 predictMultiBodyTransforms(timeStep);
40
41 }
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
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
172 void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
173 {
174 islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
175 }
176
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
187 btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld()
188 {
189 delete m_solverMultiBodyIslandCallback;
190 }
191
192 void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
193 {
194 m_multiBodyConstraintSolver = solver;
195 m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
196 btDiscreteDynamicsWorld::setConstraintSolver(solver);
197 }
198
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
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 }
216 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
217 {
218 solveExternalForces(solverInfo);
219 buildIslands();
220 solveInternalConstraints(solverInfo);
221 }
222
223 void btMultiBodyDynamicsWorld::buildIslands()
224 {
225 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
226 }
227
228 void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
229 {
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
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
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];
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
568 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
569 {
570 btDiscreteDynamicsWorld::integrateTransforms(timeStep);
571 integrateMultiBodyTransforms(timeStep);
572 }
573
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
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
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
655 void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
656 {
657 m_multiBodyConstraints.push_back(constraint);
658 }
659
660 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint)
661 {
662 m_multiBodyConstraints.remove(constraint);
663 }
664
665 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
666 {
667 constraint->debugDraw(getDebugDrawer());
668 }
669
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
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
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 }
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 }
816 void btMultiBodyDynamicsWorld::clearForces()
817 {
818 btDiscreteDynamicsWorld::clearForces();
819
820 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
821 clearMultiBodyForces();
822 #endif
823 }
824
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
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 //}
btCollisionWorld::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btCollisionWorld.h:96
btDiscreteDynamicsWorld::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btDiscreteDynamicsWorld.h:47
btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
Definition: btMultiBodyDynamicsWorld.cpp:192
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
btSimulationIslandManager.h
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:50
btDiscreteDynamicsWorld::setConstraintSolver
virtual void setConstraintSolver(btConstraintSolver *solver)
Definition: btDiscreteDynamicsWorld.cpp:1342
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:93
btUnionFind::unite
void unite(int p, int q)
Definition: btUnionFind.h:76
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition: btMultiBodyLink.h:125
btSimulationIslandManager::storeIslandActivationState
virtual void storeIslandActivationState(btCollisionWorld *world)
Definition: btSimulationIslandManager.cpp:141
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btMultiBody::predictPositionsMultiDof
void predictPositionsMultiDof(btScalar dt)
Definition: btMultiBody.cpp:1596
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
MultiBodyInplaceSolverIslandCallback::processConstraints
virtual void processConstraints(int islandId=-1)
Definition: btMultiBodyInplaceSolverIslandCallback.h:223
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
DISABLE_DEACTIVATION
#define DISABLE_DEACTIVATION
Definition: btCollisionObject.h:25
btMultiBodyDynamicsWorld::calculateSimulationIslands
virtual void calculateSimulationIslands()
Definition: btMultiBodyDynamicsWorld.cpp:42
btMultiBodyDynamicsWorld::addMultiBodyConstraint
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:655
btMultiBody::updateCollisionObjectInterpolationWorldTransforms
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2179
btMultiBodyDynamicsWorld::m_scratch_world_to_local
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local
Definition: btMultiBodyDynamicsWorld.h:41
btMultiBodyDynamicsWorld::solveExternalForces
virtual void solveExternalForces(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:281
btCollisionWorld::getDebugDrawer
virtual btIDebugDraw * getDebugDrawer()
Definition: btCollisionWorld.h:155
btMultiBody::processDeltaVeeMultiDof2
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:407
btContactSolverInfo
Definition: btContactSolverInfo.h:73
btMultibodyLink::eFixed
@ eFixed
Definition: btMultiBodyLink.h:76
btSimulationIslandManager::buildAndProcessIslands
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
Definition: btSimulationIslandManager.cpp:343
btDiscreteDynamicsWorld::m_sortedConstraints
btAlignedObjectArray< btTypedConstraint * > m_sortedConstraints
Definition: btDiscreteDynamicsWorld.h:40
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:166
btMultiBodyDynamicsWorld.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMultiBodyDynamicsWorld::m_scratch_local_origin1
btAlignedObjectArray< btVector3 > m_scratch_local_origin1
Definition: btMultiBodyDynamicsWorld.h:44
btCollisionObject::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btCollisionObject.cpp:81
btMultibodyLink::ePrismatic
@ ePrismatic
Definition: btMultiBodyLink.h:73
ACTIVE_TAG
#define ACTIVE_TAG
Definition: btCollisionObject.h:22
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:598
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btDiscreteDynamicsWorld::m_islandManager
btSimulationIslandManager * m_islandManager
Definition: btDiscreteDynamicsWorld.h:45
btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces
virtual void clearMultiBodyConstraintForces()
Definition: btMultiBodyDynamicsWorld.cpp:780
MultiBodyInplaceSolverIslandCallback::setMultiBodyConstraintSolver
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
Definition: btMultiBodyInplaceSolverIslandCallback.h:122
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:341
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:193
btMultiBodyConstraintSolver.h
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btChunk
Definition: btSerializer.h:48
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:633
btMultiBodyConstraint::getIslandIdB
virtual int getIslandIdB() const =0
btMultiBody::isPosUpdated
bool isPosUpdated() const
Definition: btMultiBody.h:588
btCollisionWorld::m_collisionObjects
btAlignedObjectArray< btCollisionObject * > m_collisionObjects
Definition: btCollisionWorld.h:88
btMultiBodyDynamicsWorld::solveInternalConstraints
virtual void solveInternalConstraints(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:228
btIDebugDraw::getDebugMode
virtual int getDebugMode() const =0
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
btMultiBodyConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)=0
btDynamicsWorld::getSolverInfo
btContactSolverInfo & getSolverInfo()
Definition: btDynamicsWorld.h:140
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2126
btMultiBodyDynamicsWorld::m_solverMultiBodyIslandCallback
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
Definition: btMultiBodyDynamicsWorld.h:38
btCollisionObject::getActivationState
int getActivationState() const
Definition: btCollisionObject.h:277
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:2230
btCollisionWorld::serializeContactManifolds
void serializeContactManifolds(btSerializer *serializer)
Definition: btCollisionWorld.cpp:1598
BT_MULTIBODY_CODE
#define BT_MULTIBODY_CODE
Definition: btSerializer.h:108
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
btDiscreteDynamicsWorld::getSimulationIslandManager
btSimulationIslandManager * getSimulationIslandManager()
Definition: btDiscreteDynamicsWorld.h:125
BT_MB_LINKCOLLIDER_CODE
#define BT_MB_LINKCOLLIDER_CODE
Definition: btSerializer.h:109
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:222
btMultiBody::isUsingRK4Integration
bool isUsingRK4Integration() const
Definition: btMultiBody.h:584
btSortConstraintOnIslandPredicate2
Definition: btMultiBodyInplaceSolverIslandCallback.h:32
btMultiBody::stepPositionsMultiDof
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
Definition: btMultiBody.cpp:1749
btMultiBodyDynamicsWorld::solveConstraints
virtual void solveConstraints(btContactSolverInfo &solverInfo)
Definition: btMultiBodyDynamicsWorld.cpp:216
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btVector4
Definition: btVector3.h:1074
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:205
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:417
btMultiBodyDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
Definition: btMultiBodyDynamicsWorld.cpp:816
btMultiBodyDynamicsWorld::m_scratch_r
btAlignedObjectArray< btScalar > m_scratch_r
Definition: btMultiBodyDynamicsWorld.h:45
MultiBodyInplaceSolverIslandCallback::setup
virtual void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
Definition: btMultiBodyInplaceSolverIslandCallback.h:104
btConstraintSolver::getSolverType
virtual btConstraintSolverType getSolverType() const =0
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:644
btMultiBody::addLinkForce
void addLinkForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:664
btMultiBodyDynamicsWorld::buildIslands
void buildIslands()
Definition: btMultiBodyDynamicsWorld.cpp:223
btDiscreteDynamicsWorld::getCollisionWorld
btCollisionWorld * getCollisionWorld()
Definition: btDiscreteDynamicsWorld.h:135
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:201
MultiBodyInplaceSolverIslandCallback
Definition: btMultiBodyInplaceSolverIslandCallback.h:66
btMultiBodyDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btMultiBodyDynamicsWorld.cpp:670
btIDebugDraw::DBG_DrawConstraints
@ DBG_DrawConstraints
Definition: btIDebugDraw.h:66
btMultiBodyDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:568
btMultiBody::checkMotionAndSleepIfRequired
void checkMotionAndSleepIfRequired(btScalar timestep)
Definition: btMultiBody.cpp:2045
btMultiBodyDynamicsWorld::m_multiBodies
btAlignedObjectArray< btMultiBody * > m_multiBodies
Definition: btMultiBodyDynamicsWorld.h:34
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btSortMultiBodyConstraintOnIslandPredicate
Definition: btMultiBodyInplaceSolverIslandCallback.h:54
btMultiBodyConstraintSolver
Definition: btMultiBodyConstraintSolver.h:30
btIDebugDraw::DBG_DrawConstraintLimits
@ DBG_DrawConstraintLimits
Definition: btIDebugDraw.h:67
btDiscreteDynamicsWorld::m_gravity
btVector3 m_gravity
Definition: btDiscreteDynamicsWorld.h:51
btDiscreteDynamicsWorld::debugDrawWorld
virtual void debugDrawWorld()
Definition: btDiscreteDynamicsWorld.cpp:269
btMultiBody::addBaseForce
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:327
btMultiBody::isAwake
bool isAwake() const
Definition: btMultiBody.h:500
btMultiBodyDynamicsWorld::m_scratch_local_origin
btAlignedObjectArray< btVector3 > m_scratch_local_origin
Definition: btMultiBodyDynamicsWorld.h:42
btMultiBody::getBaseMass
btScalar getBaseMass() const
Definition: btMultiBody.h:169
btConstraintSolver::allSolved
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
Definition: btConstraintSolver.h:51
btCollisionWorld::getNumCollisionObjects
int getNumCollisionObjects() const
Definition: btCollisionWorld.h:427
btSerializer.h
btCollisionObject::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btCollisionObject.h:673
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btDiscreteDynamicsWorld::predictUnconstraintMotion
virtual void predictUnconstraintMotion(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:1083
btCollisionObject::setDeactivationTime
void setDeactivationTime(btScalar time)
Definition: btCollisionObject.h:281
btMultiBodyConstraint::getIslandIdA
virtual int getIslandIdA() const =0
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btMultiBodyDynamicsWorld::predictUnconstraintMotion
virtual void predictUnconstraintMotion(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:36
btMultiBodyDynamicsWorld::m_multiBodyConstraintSolver
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
Definition: btMultiBodyDynamicsWorld.h:37
btIDebugDraw::DBG_DrawFrames
@ DBG_DrawFrames
Definition: btIDebugDraw.h:70
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btMultiBodyDynamicsWorld::m_sortedMultiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:36
btMultiBodyDynamicsWorld::removeMultiBodyConstraint
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:660
btDiscreteDynamicsWorld::m_predictiveManifolds
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
Definition: btDiscreteDynamicsWorld.h:69
btMultiBody::getNumPosVars
int getNumPosVars() const
Definition: btMultiBody.h:168
btSimulationIslandManager::updateActivationState
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
Definition: btSimulationIslandManager.cpp:119
btMultibodyLink::eRevolute
@ eRevolute
Definition: btMultiBodyLink.h:72
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btMultiBody
Definition: btMultiBody.h:51
btDiscreteDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btDiscreteDynamicsWorld.cpp:322
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btMultiBodyDynamicsWorld::forwardKinematics
void forwardKinematics()
Definition: btMultiBodyDynamicsWorld.cpp:208
btMultiBodyDynamicsWorld::m_scratch_v
btAlignedObjectArray< btVector3 > m_scratch_v
Definition: btMultiBodyDynamicsWorld.h:46
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:436
btSerializer::finishSerialization
virtual void finishSerialization()=0
MultiBodyInplaceSolverIslandCallback::m_islandAnalyticsData
btAlignedObjectArray< btSolverAnalyticsData > m_islandAnalyticsData
Definition: btMultiBodyInplaceSolverIslandCallback.h:84
btSerializer::startSerialization
virtual void startSerialization()=0
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:132
btMultiBodyDynamicsWorld::predictMultiBodyTransforms
void predictMultiBodyTransforms(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:621
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
output
#define output
btCollisionObject::CO_FEATHERSTONE_LINK
@ CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:153
btMultiBodyDynamicsWorld::applyGravity
virtual void applyGravity()
apply gravity, call this once per timestep
Definition: btMultiBodyDynamicsWorld.cpp:746
btAlignedObjectArray< btSolverAnalyticsData >
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:362
btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
Definition: btMultiBody.cpp:744
btMultiBody::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btMultiBody.cpp:2237
btDiscreteDynamicsWorld::integrateTransforms
virtual void integrateTransforms(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:1039
btSerializer
Definition: btSerializer.h:66
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:592
btMultiBody.h
btMultiBodyDynamicsWorld::m_scratch_m
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
Definition: btMultiBodyDynamicsWorld.h:47
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld
virtual ~btMultiBodyDynamicsWorld()
Definition: btMultiBodyDynamicsWorld.cpp:187
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:137
btMultiBodyDynamicsWorld::integrateMultiBodyTransforms
void integrateMultiBodyTransforms(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:574
btContactSolverInfoData::m_jointFeedbackInJointFrame
bool m_jointFeedbackInJointFrame
Definition: btContactSolverInfo.h:68
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
Definition: btMultiBodyDynamicsWorld.cpp:177
btMultiBody::clearVelocities
void clearVelocities()
Definition: btMultiBody.cpp:657
btBroadphaseInterface
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
Definition: btBroadphaseInterface.h:50
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:110
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:196
btIDebugDraw.h
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2083
btMultiBodyDynamicsWorld::setConstraintSolver
virtual void setConstraintSolver(btConstraintSolver *solver)
Definition: btMultiBodyDynamicsWorld.cpp:199
btQuickprof.h
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:362
btDiscreteDynamicsWorld::m_constraintSolver
btConstraintSolver * m_constraintSolver
Definition: btDiscreteDynamicsWorld.h:43
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition: btMultiBodyLink.h:58
btMultiBodyDynamicsWorld::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyDynamicsWorld.h:35
btContactSolverInfoData::m_jointFeedbackInWorldSpace
bool m_jointFeedbackInWorldSpace
Definition: btContactSolverInfo.h:67
btDiscreteDynamicsWorld::getNumConstraints
virtual int getNumConstraints() const
Definition: btDiscreteDynamicsWorld.cpp:1358
WANTS_DEACTIVATION
#define WANTS_DEACTIVATION
Definition: btCollisionObject.h:24
btDiscreteDynamicsWorld::serializeRigidBodies
void serializeRigidBodies(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1371
SOLVER_USE_2_FRICTION_DIRECTIONS
@ SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btMultiBodyDynamicsWorld::clearMultiBodyForces
virtual void clearMultiBodyForces()
Definition: btMultiBodyDynamicsWorld.cpp:788
btCollisionWorld::getDispatcher
btDispatcher * getDispatcher()
Definition: btCollisionWorld.h:132
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btCollisionWorld::serializeCollisionObjects
void serializeCollisionObjects(btSerializer *serializer)
Definition: btCollisionWorld.cpp:1568
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btMultiBodyConstraint.h
btSimulationIslandManager::getUnionFind
btUnionFind & getUnionFind()
Definition: btSimulationIslandManager.h:45
btDiscreteDynamicsWorld
btDiscreteDynamicsWorld provides discrete rigid body simulation those classes replace the obsolete Cc...
Definition: btDiscreteDynamicsWorld.h:38
btMultiBodyDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btMultiBodyDynamicsWorld.cpp:125
btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
Definition: btMultiBodyDynamicsWorld.cpp:665
btMultiBodyDynamicsWorld::serialize
virtual void serialize(btSerializer *serializer)
Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see B...
Definition: btMultiBodyDynamicsWorld.cpp:825
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btConstraintSolver::prepareSolve
virtual void prepareSolve(int, int)
Definition: btConstraintSolver.h:46
btMultiBodyDynamicsWorld::serializeMultiBodies
virtual void serializeMultiBodies(btSerializer *serializer)
Definition: btMultiBodyDynamicsWorld.cpp:842
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btCollisionConfiguration
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size,...
Definition: btCollisionConfiguration.h:27
btDiscreteDynamicsWorld::updateActivationState
virtual void updateActivationState(btScalar timeStep)
Definition: btDiscreteDynamicsWorld.cpp:594
btMultiBodyDynamicsWorld::getAnalyticsData
virtual void getAnalyticsData(btAlignedObjectArray< struct btSolverAnalyticsData > &m_islandAnalyticsData) const
Definition: btMultiBodyDynamicsWorld.cpp:172
btAlignedObjectArray::remove
void remove(const T &key)
Definition: btAlignedObjectArray.h:480
BT_MULTIBODY_SOLVER
@ BT_MULTIBODY_SOLVER
Definition: btConstraintSolver.h:37
btMultiBodyDynamicsWorld::removeMultiBody
virtual void removeMultiBody(btMultiBody *body)
Definition: btMultiBodyDynamicsWorld.cpp:31
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMultiBodyDynamicsWorld::m_scratch_world_to_local1
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local1
Definition: btMultiBodyDynamicsWorld.h:43
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btDiscreteDynamicsWorld::serializeDynamicsWorldInfo
void serializeDynamicsWorldInfo(btSerializer *serializer)
Definition: btDiscreteDynamicsWorld.cpp:1397
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btConstraintSolver
Definition: btConstraintSolver.h:42
btDiscreteDynamicsWorld::clearForces
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
Definition: btDiscreteDynamicsWorld.cpp:309
btCollisionObject::setActivationState
void setActivationState(int newState) const
Definition: btCollisionObject.cpp:61
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btMultiBodyDynamicsWorld::addMultiBody
virtual void addMultiBody(btMultiBody *body, int group=btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter)
Definition: btMultiBodyDynamicsWorld.cpp:26
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926