1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2009 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 "btDiscreteDynamicsWorld.h"
17
18 //collision detection
19 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
20 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
21 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
22 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
23 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
24 #include "LinearMath/btTransformUtil.h"
25 #include "LinearMath/btQuickprof.h"
26
27 //rigidbody & constraints
28 #include "BulletDynamics/Dynamics/btRigidBody.h"
29 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
30 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
31 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
32 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
33 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
34 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
35 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
36 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
37 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
38 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
39
40 #include "LinearMath/btIDebugDraw.h"
41 #include "BulletCollision/CollisionShapes/btSphereShape.h"
42
43 #include "BulletDynamics/Dynamics/btActionInterface.h"
44 #include "LinearMath/btQuickprof.h"
45 #include "LinearMath/btMotionState.h"
46
47 #include "LinearMath/btSerializer.h"
48
49 #if 0
50 btAlignedObjectArray<btVector3> debugContacts;
51 btAlignedObjectArray<btVector3> debugNormals;
52 int startHit=2;
53 int firstHit=startHit;
54 #endif
55
btGetConstraintIslandId(const btTypedConstraint * lhs)56 SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
57 {
58 int islandId;
59
60 const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
61 const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
62 islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
63 return islandId;
64 }
65
66 class btSortConstraintOnIslandPredicate
67 {
68 public:
operator ()(const btTypedConstraint * lhs,const btTypedConstraint * rhs) const69 bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
70 {
71 int rIslandId0, lIslandId0;
72 rIslandId0 = btGetConstraintIslandId(rhs);
73 lIslandId0 = btGetConstraintIslandId(lhs);
74 return lIslandId0 < rIslandId0;
75 }
76 };
77
78 struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
79 {
80 btContactSolverInfo* m_solverInfo;
81 btConstraintSolver* m_solver;
82 btTypedConstraint** m_sortedConstraints;
83 int m_numConstraints;
84 btIDebugDraw* m_debugDrawer;
85 btDispatcher* m_dispatcher;
86
87 btAlignedObjectArray<btCollisionObject*> m_bodies;
88 btAlignedObjectArray<btPersistentManifold*> m_manifolds;
89 btAlignedObjectArray<btTypedConstraint*> m_constraints;
90
InplaceSolverIslandCallbackInplaceSolverIslandCallback91 InplaceSolverIslandCallback(
92 btConstraintSolver* solver,
93 btStackAlloc* stackAlloc,
94 btDispatcher* dispatcher)
95 : m_solverInfo(NULL),
96 m_solver(solver),
97 m_sortedConstraints(NULL),
98 m_numConstraints(0),
99 m_debugDrawer(NULL),
100 m_dispatcher(dispatcher)
101 {
102 }
103
operator =InplaceSolverIslandCallback104 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
105 {
106 btAssert(0);
107 (void)other;
108 return *this;
109 }
110
setupInplaceSolverIslandCallback111 SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer)
112 {
113 btAssert(solverInfo);
114 m_solverInfo = solverInfo;
115 m_sortedConstraints = sortedConstraints;
116 m_numConstraints = numConstraints;
117 m_debugDrawer = debugDrawer;
118 m_bodies.resize(0);
119 m_manifolds.resize(0);
120 m_constraints.resize(0);
121 }
122
processIslandInplaceSolverIslandCallback123 virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
124 {
125 if (islandId < 0)
126 {
127 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
128 m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, &m_sortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
129 }
130 else
131 {
132 //also add all non-contact constraints/joints for this island
133 btTypedConstraint** startConstraint = 0;
134 int numCurConstraints = 0;
135 int i;
136
137 //find the first constraint for this island
138 for (i = 0; i < m_numConstraints; i++)
139 {
140 if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
141 {
142 startConstraint = &m_sortedConstraints[i];
143 break;
144 }
145 }
146 //count the number of constraints in this island
147 for (; i < m_numConstraints; i++)
148 {
149 if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
150 {
151 numCurConstraints++;
152 }
153 }
154
155 if (m_solverInfo->m_minimumSolverBatchSize <= 1)
156 {
157 m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
158 }
159 else
160 {
161 for (i = 0; i < numBodies; i++)
162 m_bodies.push_back(bodies[i]);
163 for (i = 0; i < numManifolds; i++)
164 m_manifolds.push_back(manifolds[i]);
165 for (i = 0; i < numCurConstraints; i++)
166 m_constraints.push_back(startConstraint[i]);
167 if ((m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
168 {
169 processConstraints();
170 }
171 else
172 {
173 //printf("deferred\n");
174 }
175 }
176 }
177 }
processConstraintsInplaceSolverIslandCallback178 void processConstraints()
179 {
180 btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
181 btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
182 btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
183
184 m_solver->solveGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
185 m_bodies.resize(0);
186 m_manifolds.resize(0);
187 m_constraints.resize(0);
188 }
189 };
190
btDiscreteDynamicsWorld(btDispatcher * dispatcher,btBroadphaseInterface * pairCache,btConstraintSolver * constraintSolver,btCollisionConfiguration * collisionConfiguration)191 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
192 : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
193 m_sortedConstraints(),
194 m_solverIslandCallback(NULL),
195 m_constraintSolver(constraintSolver),
196 m_gravity(0, -10, 0),
197 m_localTime(0),
198 m_fixedTimeStep(0),
199 m_synchronizeAllMotionStates(false),
200 m_applySpeculativeContactRestitution(false),
201 m_profileTimings(0),
202 m_latencyMotionStateInterpolation(true)
203
204 {
205 if (!m_constraintSolver)
206 {
207 void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver), 16);
208 m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
209 m_ownsConstraintSolver = true;
210 }
211 else
212 {
213 m_ownsConstraintSolver = false;
214 }
215
216 {
217 void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager), 16);
218 m_islandManager = new (mem) btSimulationIslandManager();
219 }
220
221 m_ownsIslandManager = true;
222
223 {
224 void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback), 16);
225 m_solverIslandCallback = new (mem) InplaceSolverIslandCallback(m_constraintSolver, 0, dispatcher);
226 }
227 }
228
~btDiscreteDynamicsWorld()229 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
230 {
231 //only delete it when we created it
232 if (m_ownsIslandManager)
233 {
234 m_islandManager->~btSimulationIslandManager();
235 btAlignedFree(m_islandManager);
236 }
237 if (m_solverIslandCallback)
238 {
239 m_solverIslandCallback->~InplaceSolverIslandCallback();
240 btAlignedFree(m_solverIslandCallback);
241 }
242 if (m_ownsConstraintSolver)
243 {
244 m_constraintSolver->~btConstraintSolver();
245 btAlignedFree(m_constraintSolver);
246 }
247 }
248
saveKinematicState(btScalar timeStep)249 void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
250 {
251 ///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
252 ///to switch status _after_ adding kinematic objects to the world
253 ///fix it for Bullet 3.x release
254 for (int i = 0; i < m_collisionObjects.size(); i++)
255 {
256 btCollisionObject* colObj = m_collisionObjects[i];
257 btRigidBody* body = btRigidBody::upcast(colObj);
258 if (body && body->getActivationState() != ISLAND_SLEEPING)
259 {
260 if (body->isKinematicObject())
261 {
262 //to calculate velocities next frame
263 body->saveKinematicState(timeStep);
264 }
265 }
266 }
267 }
268
debugDrawWorld()269 void btDiscreteDynamicsWorld::debugDrawWorld()
270 {
271 BT_PROFILE("debugDrawWorld");
272
273 btCollisionWorld::debugDrawWorld();
274
275 bool drawConstraints = false;
276 if (getDebugDrawer())
277 {
278 int mode = getDebugDrawer()->getDebugMode();
279 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
280 {
281 drawConstraints = true;
282 }
283 }
284 if (drawConstraints)
285 {
286 for (int i = getNumConstraints() - 1; i >= 0; i--)
287 {
288 btTypedConstraint* constraint = getConstraint(i);
289 debugDrawConstraint(constraint);
290 }
291 }
292
293 if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
294 {
295 int i;
296
297 if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
298 {
299 for (i = 0; i < m_actions.size(); i++)
300 {
301 m_actions[i]->debugDraw(m_debugDrawer);
302 }
303 }
304 }
305 if (getDebugDrawer())
306 getDebugDrawer()->flushLines();
307 }
308
clearForces()309 void btDiscreteDynamicsWorld::clearForces()
310 {
311 ///@todo: iterate over awake simulation islands!
312 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
313 {
314 btRigidBody* body = m_nonStaticRigidBodies[i];
315 //need to check if next line is ok
316 //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
317 body->clearForces();
318 }
319 }
320
321 ///apply gravity, call this once per timestep
applyGravity()322 void btDiscreteDynamicsWorld::applyGravity()
323 {
324 ///@todo: iterate over awake simulation islands!
325 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
326 {
327 btRigidBody* body = m_nonStaticRigidBodies[i];
328 if (body->isActive())
329 {
330 body->applyGravity();
331 }
332 }
333 }
334
synchronizeSingleMotionState(btRigidBody * body)335 void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
336 {
337 btAssert(body);
338
339 if (body->getMotionState() && !body->isStaticOrKinematicObject())
340 {
341 //we need to call the update at least once, even for sleeping objects
342 //otherwise the 'graphics' transform never updates properly
343 ///@todo: add 'dirty' flag
344 //if (body->getActivationState() != ISLAND_SLEEPING)
345 {
346 btTransform interpolatedTransform;
347 btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
348 body->getInterpolationLinearVelocity(), body->getInterpolationAngularVelocity(),
349 (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime * body->getHitFraction(),
350 interpolatedTransform);
351 body->getMotionState()->setWorldTransform(interpolatedTransform);
352 }
353 }
354 }
355
synchronizeMotionStates()356 void btDiscreteDynamicsWorld::synchronizeMotionStates()
357 {
358 // BT_PROFILE("synchronizeMotionStates");
359 if (m_synchronizeAllMotionStates)
360 {
361 //iterate over all collision objects
362 for (int i = 0; i < m_collisionObjects.size(); i++)
363 {
364 btCollisionObject* colObj = m_collisionObjects[i];
365 btRigidBody* body = btRigidBody::upcast(colObj);
366 if (body)
367 synchronizeSingleMotionState(body);
368 }
369 }
370 else
371 {
372 //iterate over all active rigid bodies
373 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
374 {
375 btRigidBody* body = m_nonStaticRigidBodies[i];
376 if (body->isActive())
377 synchronizeSingleMotionState(body);
378 }
379 }
380 }
381
stepSimulation(btScalar timeStep,int maxSubSteps,btScalar fixedTimeStep)382 int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
383 {
384 startProfiling(timeStep);
385
386 int numSimulationSubSteps = 0;
387
388 if (maxSubSteps)
389 {
390 //fixed timestep with interpolation
391 m_fixedTimeStep = fixedTimeStep;
392 m_localTime += timeStep;
393 if (m_localTime >= fixedTimeStep)
394 {
395 numSimulationSubSteps = int(m_localTime / fixedTimeStep);
396 m_localTime -= numSimulationSubSteps * fixedTimeStep;
397 }
398 }
399 else
400 {
401 //variable timestep
402 fixedTimeStep = timeStep;
403 m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
404 m_fixedTimeStep = 0;
405 if (btFuzzyZero(timeStep))
406 {
407 numSimulationSubSteps = 0;
408 maxSubSteps = 0;
409 }
410 else
411 {
412 numSimulationSubSteps = 1;
413 maxSubSteps = 1;
414 }
415 }
416
417 //process some debugging flags
418 if (getDebugDrawer())
419 {
420 btIDebugDraw* debugDrawer = getDebugDrawer();
421 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
422 }
423 if (numSimulationSubSteps)
424 {
425 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
426 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
427
428 saveKinematicState(fixedTimeStep * clampedSimulationSteps);
429
430 applyGravity();
431
432 for (int i = 0; i < clampedSimulationSteps; i++)
433 {
434 internalSingleStepSimulation(fixedTimeStep);
435 synchronizeMotionStates();
436 }
437 }
438 else
439 {
440 synchronizeMotionStates();
441 }
442
443 clearForces();
444
445 #ifndef BT_NO_PROFILE
446 CProfileManager::Increment_Frame_Counter();
447 #endif //BT_NO_PROFILE
448
449 return numSimulationSubSteps;
450 }
451
internalSingleStepSimulation(btScalar timeStep)452 void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
453 {
454 BT_PROFILE("internalSingleStepSimulation");
455
456 if (0 != m_internalPreTickCallback)
457 {
458 (*m_internalPreTickCallback)(this, timeStep);
459 }
460
461 ///apply gravity, predict motion
462 predictUnconstraintMotion(timeStep);
463
464 btDispatcherInfo& dispatchInfo = getDispatchInfo();
465
466 dispatchInfo.m_timeStep = timeStep;
467 dispatchInfo.m_stepCount = 0;
468 dispatchInfo.m_debugDraw = getDebugDrawer();
469
470 createPredictiveContacts(timeStep);
471
472 ///perform collision detection
473 performDiscreteCollisionDetection();
474
475 calculateSimulationIslands();
476
477 getSolverInfo().m_timeStep = timeStep;
478
479 ///solve contact and other joint constraints
480 solveConstraints(getSolverInfo());
481
482 ///CallbackTriggers();
483
484 ///integrate transforms
485
486 integrateTransforms(timeStep);
487
488 ///update vehicle simulation
489 updateActions(timeStep);
490
491 updateActivationState(timeStep);
492
493 if (0 != m_internalTickCallback)
494 {
495 (*m_internalTickCallback)(this, timeStep);
496 }
497 }
498
setGravity(const btVector3 & gravity)499 void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
500 {
501 m_gravity = gravity;
502 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
503 {
504 btRigidBody* body = m_nonStaticRigidBodies[i];
505 if (body->isActive() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
506 {
507 body->setGravity(gravity);
508 }
509 }
510 }
511
getGravity() const512 btVector3 btDiscreteDynamicsWorld::getGravity() const
513 {
514 return m_gravity;
515 }
516
addCollisionObject(btCollisionObject * collisionObject,int collisionFilterGroup,int collisionFilterMask)517 void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
518 {
519 btCollisionWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
520 }
521
removeCollisionObject(btCollisionObject * collisionObject)522 void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
523 {
524 btRigidBody* body = btRigidBody::upcast(collisionObject);
525 if (body)
526 removeRigidBody(body);
527 else
528 btCollisionWorld::removeCollisionObject(collisionObject);
529 }
530
removeRigidBody(btRigidBody * body)531 void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
532 {
533 m_nonStaticRigidBodies.remove(body);
534 btCollisionWorld::removeCollisionObject(body);
535 }
536
addRigidBody(btRigidBody * body)537 void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
538 {
539 if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
540 {
541 body->setGravity(m_gravity);
542 }
543
544 if (body->getCollisionShape())
545 {
546 if (!body->isStaticObject())
547 {
548 m_nonStaticRigidBodies.push_back(body);
549 }
550 else
551 {
552 body->setActivationState(ISLAND_SLEEPING);
553 }
554
555 bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
556 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
557 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
558
559 addCollisionObject(body, collisionFilterGroup, collisionFilterMask);
560 }
561 }
562
addRigidBody(btRigidBody * body,int group,int mask)563 void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
564 {
565 if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
566 {
567 body->setGravity(m_gravity);
568 }
569
570 if (body->getCollisionShape())
571 {
572 if (!body->isStaticObject())
573 {
574 m_nonStaticRigidBodies.push_back(body);
575 }
576 else
577 {
578 body->setActivationState(ISLAND_SLEEPING);
579 }
580 addCollisionObject(body, group, mask);
581 }
582 }
583
updateActions(btScalar timeStep)584 void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
585 {
586 BT_PROFILE("updateActions");
587
588 for (int i = 0; i < m_actions.size(); i++)
589 {
590 m_actions[i]->updateAction(this, timeStep);
591 }
592 }
593
updateActivationState(btScalar timeStep)594 void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
595 {
596 BT_PROFILE("updateActivationState");
597
598 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
599 {
600 btRigidBody* body = m_nonStaticRigidBodies[i];
601 if (body)
602 {
603 body->updateDeactivation(timeStep);
604
605 if (body->wantsSleeping())
606 {
607 if (body->isStaticOrKinematicObject())
608 {
609 body->setActivationState(ISLAND_SLEEPING);
610 }
611 else
612 {
613 if (body->getActivationState() == ACTIVE_TAG)
614 body->setActivationState(WANTS_DEACTIVATION);
615 if (body->getActivationState() == ISLAND_SLEEPING)
616 {
617 body->setAngularVelocity(btVector3(0, 0, 0));
618 body->setLinearVelocity(btVector3(0, 0, 0));
619 }
620 }
621 }
622 else
623 {
624 if (body->getActivationState() != DISABLE_DEACTIVATION)
625 body->setActivationState(ACTIVE_TAG);
626 }
627 }
628 }
629 }
630
addConstraint(btTypedConstraint * constraint,bool disableCollisionsBetweenLinkedBodies)631 void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
632 {
633 m_constraints.push_back(constraint);
634 //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
635 btAssert(&constraint->getRigidBodyA() != &constraint->getRigidBodyB());
636
637 if (disableCollisionsBetweenLinkedBodies)
638 {
639 constraint->getRigidBodyA().addConstraintRef(constraint);
640 constraint->getRigidBodyB().addConstraintRef(constraint);
641 }
642 }
643
removeConstraint(btTypedConstraint * constraint)644 void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
645 {
646 m_constraints.remove(constraint);
647 constraint->getRigidBodyA().removeConstraintRef(constraint);
648 constraint->getRigidBodyB().removeConstraintRef(constraint);
649 }
650
addAction(btActionInterface * action)651 void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
652 {
653 m_actions.push_back(action);
654 }
655
removeAction(btActionInterface * action)656 void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
657 {
658 m_actions.remove(action);
659 }
660
addVehicle(btActionInterface * vehicle)661 void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
662 {
663 addAction(vehicle);
664 }
665
removeVehicle(btActionInterface * vehicle)666 void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
667 {
668 removeAction(vehicle);
669 }
670
addCharacter(btActionInterface * character)671 void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
672 {
673 addAction(character);
674 }
675
removeCharacter(btActionInterface * character)676 void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
677 {
678 removeAction(character);
679 }
680
solveConstraints(btContactSolverInfo & solverInfo)681 void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
682 {
683 BT_PROFILE("solveConstraints");
684
685 m_sortedConstraints.resize(m_constraints.size());
686 int i;
687 for (i = 0; i < getNumConstraints(); i++)
688 {
689 m_sortedConstraints[i] = m_constraints[i];
690 }
691
692 // btAssert(0);
693
694 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
695
696 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
697
698 m_solverIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), getDebugDrawer());
699 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
700
701 /// solve all the constraints for this island
702 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverIslandCallback);
703
704 m_solverIslandCallback->processConstraints();
705
706 m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
707 }
708
calculateSimulationIslands()709 void btDiscreteDynamicsWorld::calculateSimulationIslands()
710 {
711 BT_PROFILE("calculateSimulationIslands");
712
713 getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
714
715 {
716 //merge islands based on speculative contact manifolds too
717 for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
718 {
719 btPersistentManifold* manifold = m_predictiveManifolds[i];
720
721 const btCollisionObject* colObj0 = manifold->getBody0();
722 const btCollisionObject* colObj1 = manifold->getBody1();
723
724 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
725 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
726 {
727 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
728 }
729 }
730 }
731
732 {
733 int i;
734 int numConstraints = int(m_constraints.size());
735 for (i = 0; i < numConstraints; i++)
736 {
737 btTypedConstraint* constraint = m_constraints[i];
738 if (constraint->isEnabled())
739 {
740 const btRigidBody* colObj0 = &constraint->getRigidBodyA();
741 const btRigidBody* colObj1 = &constraint->getRigidBodyB();
742
743 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
744 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
745 {
746 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
747 }
748 }
749 }
750 }
751
752 //Store the island id in each body
753 getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
754 }
755
756 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
757 {
758 public:
759 btCollisionObject* m_me;
760 btScalar m_allowedPenetration;
761 btOverlappingPairCache* m_pairCache;
762 btDispatcher* m_dispatcher;
763
764 public:
btClosestNotMeConvexResultCallback(btCollisionObject * me,const btVector3 & fromA,const btVector3 & toA,btOverlappingPairCache * pairCache,btDispatcher * dispatcher)765 btClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA, toA),
766 m_me(me),
767 m_allowedPenetration(0.0f),
768 m_pairCache(pairCache),
769 m_dispatcher(dispatcher)
770 {
771 }
772
addSingleResult(btCollisionWorld::LocalConvexResult & convexResult,bool normalInWorldSpace)773 virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
774 {
775 if (convexResult.m_hitCollisionObject == m_me)
776 return 1.0f;
777
778 //ignore result if there is no contact response
779 if (!convexResult.m_hitCollisionObject->hasContactResponse())
780 return 1.0f;
781
782 btVector3 linVelA, linVelB;
783 linVelA = m_convexToWorld - m_convexFromWorld;
784 linVelB = btVector3(0, 0, 0); //toB.getOrigin()-fromB.getOrigin();
785
786 btVector3 relativeVelocity = (linVelA - linVelB);
787 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
788 if (convexResult.m_hitNormalLocal.dot(relativeVelocity) >= -m_allowedPenetration)
789 return 1.f;
790
791 return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
792 }
793
needsCollision(btBroadphaseProxy * proxy0) const794 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
795 {
796 //don't collide with itself
797 if (proxy0->m_clientObject == m_me)
798 return false;
799
800 ///don't do CCD when the collision filters are not matching
801 if (!ClosestConvexResultCallback::needsCollision(proxy0))
802 return false;
803 if (m_pairCache->getOverlapFilterCallback()) {
804 btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle();
805 bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1);
806 if (!collides)
807 {
808 return false;
809 }
810 }
811
812 btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
813
814 if (!m_dispatcher->needsCollision(m_me, otherObj))
815 return false;
816
817 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
818 if (m_dispatcher->needsResponse(m_me, otherObj))
819 {
820 #if 0
821 ///don't do CCD when there are already contact points (touching contact/penetration)
822 btAlignedObjectArray<btPersistentManifold*> manifoldArray;
823 btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
824 if (collisionPair)
825 {
826 if (collisionPair->m_algorithm)
827 {
828 manifoldArray.resize(0);
829 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
830 for (int j=0;j<manifoldArray.size();j++)
831 {
832 btPersistentManifold* manifold = manifoldArray[j];
833 if (manifold->getNumContacts()>0)
834 return false;
835 }
836 }
837 }
838 #endif
839 return true;
840 }
841
842 return false;
843 }
844 };
845
846 ///internal debugging variable. this value shouldn't be too high
847 int gNumClampedCcdMotions = 0;
848
createPredictiveContactsInternal(btRigidBody ** bodies,int numBodies,btScalar timeStep)849 void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
850 {
851 btTransform predictedTrans;
852 for (int i = 0; i < numBodies; i++)
853 {
854 btRigidBody* body = bodies[i];
855 body->setHitFraction(1.f);
856
857 if (body->isActive() && (!body->isStaticOrKinematicObject()))
858 {
859 body->predictIntegratedTransform(timeStep, predictedTrans);
860
861 btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
862
863 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
864 {
865 BT_PROFILE("predictive convexSweepTest");
866 if (body->getCollisionShape()->isConvex())
867 {
868 gNumClampedCcdMotions++;
869 #ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY
870 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
871 {
872 public:
873 StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
874 {
875 }
876
877 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
878 {
879 btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
880 if (!otherObj->isStaticOrKinematicObject())
881 return false;
882 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
883 }
884 };
885
886 StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
887 #else
888 btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
889 #endif
890 //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
891 btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
892 sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
893
894 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
895 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
896 btTransform modifiedPredictedTrans = predictedTrans;
897 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
898
899 convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
900 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
901 {
902 btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction;
903 btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
904
905 btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
906 btMutexLock(&m_predictiveManifoldsMutex);
907 m_predictiveManifolds.push_back(manifold);
908 btMutexUnlock(&m_predictiveManifoldsMutex);
909
910 btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec;
911 btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB;
912
913 btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance);
914
915 bool isPredictive = true;
916 int index = manifold->addManifoldPoint(newPoint, isPredictive);
917 btManifoldPoint& pt = manifold->getContactPoint(index);
918 pt.m_combinedRestitution = 0;
919 pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject);
920 pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
921 pt.m_positionWorldOnB = worldPointB;
922 }
923 }
924 }
925 }
926 }
927 }
928
releasePredictiveContacts()929 void btDiscreteDynamicsWorld::releasePredictiveContacts()
930 {
931 BT_PROFILE("release predictive contact manifolds");
932
933 for (int i = 0; i < m_predictiveManifolds.size(); i++)
934 {
935 btPersistentManifold* manifold = m_predictiveManifolds[i];
936 this->m_dispatcher1->releaseManifold(manifold);
937 }
938 m_predictiveManifolds.clear();
939 }
940
createPredictiveContacts(btScalar timeStep)941 void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
942 {
943 BT_PROFILE("createPredictiveContacts");
944 releasePredictiveContacts();
945 if (m_nonStaticRigidBodies.size() > 0)
946 {
947 createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
948 }
949 }
950
integrateTransformsInternal(btRigidBody ** bodies,int numBodies,btScalar timeStep)951 void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
952 {
953 btTransform predictedTrans;
954 for (int i = 0; i < numBodies; i++)
955 {
956 btRigidBody* body = bodies[i];
957 body->setHitFraction(1.f);
958
959 if (body->isActive() && (!body->isStaticOrKinematicObject()))
960 {
961 body->predictIntegratedTransform(timeStep, predictedTrans);
962
963 btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
964
965 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
966 {
967 BT_PROFILE("CCD motion clamping");
968 if (body->getCollisionShape()->isConvex())
969 {
970 gNumClampedCcdMotions++;
971 #ifdef USE_STATIC_ONLY
972 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
973 {
974 public:
975 StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
976 {
977 }
978
979 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
980 {
981 btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
982 if (!otherObj->isStaticOrKinematicObject())
983 return false;
984 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
985 }
986 };
987
988 StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
989 #else
990 btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
991 #endif
992 //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
993 btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
994 sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
995
996 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
997 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
998 btTransform modifiedPredictedTrans = predictedTrans;
999 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
1000
1001 convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
1002 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
1003 {
1004 //printf("clamped integration to hit fraction = %f\n",fraction);
1005 body->setHitFraction(sweepResults.m_closestHitFraction);
1006 body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans);
1007 body->setHitFraction(0.f);
1008 body->proceedToTransform(predictedTrans);
1009
1010 #if 0
1011 btVector3 linVel = body->getLinearVelocity();
1012
1013 btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
1014 btScalar maxSpeedSqr = maxSpeed*maxSpeed;
1015 if (linVel.length2()>maxSpeedSqr)
1016 {
1017 linVel.normalize();
1018 linVel*= maxSpeed;
1019 body->setLinearVelocity(linVel);
1020 btScalar ms2 = body->getLinearVelocity().length2();
1021 body->predictIntegratedTransform(timeStep, predictedTrans);
1022
1023 btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
1024 btScalar smt = body->getCcdSquareMotionThreshold();
1025 printf("sm2=%f\n",sm2);
1026 }
1027 #else
1028
1029 //don't apply the collision response right now, it will happen next frame
1030 //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
1031 //btScalar appliedImpulse = 0.f;
1032 //btScalar depth = 0.f;
1033 //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
1034
1035 #endif
1036
1037 continue;
1038 }
1039 }
1040 }
1041
1042 body->proceedToTransform(predictedTrans);
1043 }
1044 }
1045 }
1046
integrateTransforms(btScalar timeStep)1047 void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
1048 {
1049 BT_PROFILE("integrateTransforms");
1050 if (m_nonStaticRigidBodies.size() > 0)
1051 {
1052 integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
1053 }
1054
1055 ///this should probably be switched on by default, but it is not well tested yet
1056 if (m_applySpeculativeContactRestitution)
1057 {
1058 BT_PROFILE("apply speculative contact restitution");
1059 for (int i = 0; i < m_predictiveManifolds.size(); i++)
1060 {
1061 btPersistentManifold* manifold = m_predictiveManifolds[i];
1062 btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
1063 btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
1064
1065 for (int p = 0; p < manifold->getNumContacts(); p++)
1066 {
1067 const btManifoldPoint& pt = manifold->getContactPoint(p);
1068 btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1);
1069
1070 if (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f)
1071 //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
1072 {
1073 btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution;
1074
1075 const btVector3& pos1 = pt.getPositionWorldOnA();
1076 const btVector3& pos2 = pt.getPositionWorldOnB();
1077
1078 btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
1079 btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
1080
1081 if (body0)
1082 body0->applyImpulse(imp, rel_pos0);
1083 if (body1)
1084 body1->applyImpulse(-imp, rel_pos1);
1085 }
1086 }
1087 }
1088 }
1089 }
1090
predictUnconstraintMotion(btScalar timeStep)1091 void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
1092 {
1093 BT_PROFILE("predictUnconstraintMotion");
1094 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
1095 {
1096 btRigidBody* body = m_nonStaticRigidBodies[i];
1097 if (!body->isStaticOrKinematicObject())
1098 {
1099 //don't integrate/update velocities here, it happens in the constraint solver
1100
1101 body->applyDamping(timeStep);
1102
1103 body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
1104 }
1105 }
1106 }
1107
startProfiling(btScalar timeStep)1108 void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
1109 {
1110 (void)timeStep;
1111
1112 #ifndef BT_NO_PROFILE
1113 CProfileManager::Reset();
1114 #endif //BT_NO_PROFILE
1115 }
1116
debugDrawConstraint(btTypedConstraint * constraint)1117 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
1118 {
1119 bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
1120 bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
1121 btScalar dbgDrawSize = constraint->getDbgDrawSize();
1122 if (dbgDrawSize <= btScalar(0.f))
1123 {
1124 return;
1125 }
1126
1127 switch (constraint->getConstraintType())
1128 {
1129 case POINT2POINT_CONSTRAINT_TYPE:
1130 {
1131 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
1132 btTransform tr;
1133 tr.setIdentity();
1134 btVector3 pivot = p2pC->getPivotInA();
1135 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
1136 tr.setOrigin(pivot);
1137 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1138 // that ideally should draw the same frame
1139 pivot = p2pC->getPivotInB();
1140 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
1141 tr.setOrigin(pivot);
1142 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1143 }
1144 break;
1145 case HINGE_CONSTRAINT_TYPE:
1146 {
1147 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
1148 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
1149 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1150 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
1151 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1152 btScalar minAng = pHinge->getLowerLimit();
1153 btScalar maxAng = pHinge->getUpperLimit();
1154 if (minAng == maxAng)
1155 {
1156 break;
1157 }
1158 bool drawSect = true;
1159 if (!pHinge->hasLimit())
1160 {
1161 minAng = btScalar(0.f);
1162 maxAng = SIMD_2_PI;
1163 drawSect = false;
1164 }
1165 if (drawLimits)
1166 {
1167 btVector3& center = tr.getOrigin();
1168 btVector3 normal = tr.getBasis().getColumn(2);
1169 btVector3 axis = tr.getBasis().getColumn(0);
1170 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect);
1171 }
1172 }
1173 break;
1174 case CONETWIST_CONSTRAINT_TYPE:
1175 {
1176 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
1177 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1178 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1179 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1180 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1181 if (drawLimits)
1182 {
1183 //const btScalar length = btScalar(5);
1184 const btScalar length = dbgDrawSize;
1185 static int nSegments = 8 * 4;
1186 btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments);
1187 btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
1188 pPrev = tr * pPrev;
1189 for (int i = 0; i < nSegments; i++)
1190 {
1191 fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments);
1192 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
1193 pCur = tr * pCur;
1194 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0));
1195
1196 if (i % (nSegments / 8) == 0)
1197 getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0, 0, 0));
1198
1199 pPrev = pCur;
1200 }
1201 btScalar tws = pCT->getTwistSpan();
1202 btScalar twa = pCT->getTwistAngle();
1203 bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
1204 if (useFrameB)
1205 {
1206 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1207 }
1208 else
1209 {
1210 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1211 }
1212 btVector3 pivot = tr.getOrigin();
1213 btVector3 normal = tr.getBasis().getColumn(0);
1214 btVector3 axis1 = tr.getBasis().getColumn(1);
1215 getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true);
1216 }
1217 }
1218 break;
1219 case D6_SPRING_CONSTRAINT_TYPE:
1220 case D6_CONSTRAINT_TYPE:
1221 {
1222 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
1223 btTransform tr = p6DOF->getCalculatedTransformA();
1224 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1225 tr = p6DOF->getCalculatedTransformB();
1226 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1227 if (drawLimits)
1228 {
1229 tr = p6DOF->getCalculatedTransformA();
1230 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1231 btVector3 up = tr.getBasis().getColumn(2);
1232 btVector3 axis = tr.getBasis().getColumn(0);
1233 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1234 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1235 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1236 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1237 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
1238 axis = tr.getBasis().getColumn(1);
1239 btScalar ay = p6DOF->getAngle(1);
1240 btScalar az = p6DOF->getAngle(2);
1241 btScalar cy = btCos(ay);
1242 btScalar sy = btSin(ay);
1243 btScalar cz = btCos(az);
1244 btScalar sz = btSin(az);
1245 btVector3 ref;
1246 ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
1247 ref[1] = -sz * axis[0] + cz * axis[1];
1248 ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
1249 tr = p6DOF->getCalculatedTransformB();
1250 btVector3 normal = -tr.getBasis().getColumn(0);
1251 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1252 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1253 if (minFi > maxFi)
1254 {
1255 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
1256 }
1257 else if (minFi < maxFi)
1258 {
1259 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
1260 }
1261 tr = p6DOF->getCalculatedTransformA();
1262 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1263 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1264 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
1265 }
1266 }
1267 break;
1268 ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
1269 case D6_SPRING_2_CONSTRAINT_TYPE:
1270 {
1271 {
1272 btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
1273 btTransform tr = p6DOF->getCalculatedTransformA();
1274 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1275 tr = p6DOF->getCalculatedTransformB();
1276 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1277 if (drawLimits)
1278 {
1279 tr = p6DOF->getCalculatedTransformA();
1280 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1281 btVector3 up = tr.getBasis().getColumn(2);
1282 btVector3 axis = tr.getBasis().getColumn(0);
1283 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1284 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1285 if (minTh <= maxTh)
1286 {
1287 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1288 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1289 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
1290 }
1291 axis = tr.getBasis().getColumn(1);
1292 btScalar ay = p6DOF->getAngle(1);
1293 btScalar az = p6DOF->getAngle(2);
1294 btScalar cy = btCos(ay);
1295 btScalar sy = btSin(ay);
1296 btScalar cz = btCos(az);
1297 btScalar sz = btSin(az);
1298 btVector3 ref;
1299 ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
1300 ref[1] = -sz * axis[0] + cz * axis[1];
1301 ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
1302 tr = p6DOF->getCalculatedTransformB();
1303 btVector3 normal = -tr.getBasis().getColumn(0);
1304 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1305 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1306 if (minFi > maxFi)
1307 {
1308 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
1309 }
1310 else if (minFi < maxFi)
1311 {
1312 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
1313 }
1314 tr = p6DOF->getCalculatedTransformA();
1315 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1316 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1317 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
1318 }
1319 }
1320 break;
1321 }
1322 case SLIDER_CONSTRAINT_TYPE:
1323 {
1324 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
1325 btTransform tr = pSlider->getCalculatedTransformA();
1326 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1327 tr = pSlider->getCalculatedTransformB();
1328 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1329 if (drawLimits)
1330 {
1331 btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
1332 btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
1333 btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
1334 getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
1335 btVector3 normal = tr.getBasis().getColumn(0);
1336 btVector3 axis = tr.getBasis().getColumn(1);
1337 btScalar a_min = pSlider->getLowerAngLimit();
1338 btScalar a_max = pSlider->getUpperAngLimit();
1339 const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
1340 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true);
1341 }
1342 }
1343 break;
1344 default:
1345 break;
1346 }
1347 return;
1348 }
1349
setConstraintSolver(btConstraintSolver * solver)1350 void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
1351 {
1352 if (m_ownsConstraintSolver)
1353 {
1354 btAlignedFree(m_constraintSolver);
1355 }
1356 m_ownsConstraintSolver = false;
1357 m_constraintSolver = solver;
1358 m_solverIslandCallback->m_solver = solver;
1359 }
1360
getConstraintSolver()1361 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
1362 {
1363 return m_constraintSolver;
1364 }
1365
getNumConstraints() const1366 int btDiscreteDynamicsWorld::getNumConstraints() const
1367 {
1368 return int(m_constraints.size());
1369 }
getConstraint(int index)1370 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
1371 {
1372 return m_constraints[index];
1373 }
getConstraint(int index) const1374 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
1375 {
1376 return m_constraints[index];
1377 }
1378
serializeRigidBodies(btSerializer * serializer)1379 void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
1380 {
1381 int i;
1382 //serialize all collision objects
1383 for (i = 0; i < m_collisionObjects.size(); i++)
1384 {
1385 btCollisionObject* colObj = m_collisionObjects[i];
1386 if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
1387 {
1388 int len = colObj->calculateSerializeBufferSize();
1389 btChunk* chunk = serializer->allocate(len, 1);
1390 const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
1391 serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj);
1392 }
1393 }
1394
1395 for (i = 0; i < m_constraints.size(); i++)
1396 {
1397 btTypedConstraint* constraint = m_constraints[i];
1398 int size = constraint->calculateSerializeBufferSize();
1399 btChunk* chunk = serializer->allocate(size, 1);
1400 const char* structType = constraint->serialize(chunk->m_oldPtr, serializer);
1401 serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint);
1402 }
1403 }
1404
serializeDynamicsWorldInfo(btSerializer * serializer)1405 void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
1406 {
1407 #ifdef BT_USE_DOUBLE_PRECISION
1408 int len = sizeof(btDynamicsWorldDoubleData);
1409 btChunk* chunk = serializer->allocate(len, 1);
1410 btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
1411 #else //BT_USE_DOUBLE_PRECISION
1412 int len = sizeof(btDynamicsWorldFloatData);
1413 btChunk* chunk = serializer->allocate(len, 1);
1414 btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
1415 #endif //BT_USE_DOUBLE_PRECISION
1416
1417 memset(worldInfo, 0x00, len);
1418
1419 m_gravity.serialize(worldInfo->m_gravity);
1420 worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
1421 worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
1422 worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
1423 worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
1424
1425 worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
1426 worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
1427 worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
1428 worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
1429
1430 worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
1431 worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
1432 worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
1433 worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
1434
1435 worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
1436 worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
1437 worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
1438 worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
1439
1440 worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
1441 worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
1442 worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
1443 worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
1444
1445 worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
1446
1447
1448 #ifdef BT_USE_DOUBLE_PRECISION
1449 const char* structType = "btDynamicsWorldDoubleData";
1450 #else //BT_USE_DOUBLE_PRECISION
1451 const char* structType = "btDynamicsWorldFloatData";
1452 #endif //BT_USE_DOUBLE_PRECISION
1453 serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo);
1454 }
1455
serialize(btSerializer * serializer)1456 void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
1457 {
1458 serializer->startSerialization();
1459
1460 serializeDynamicsWorldInfo(serializer);
1461
1462 serializeCollisionObjects(serializer);
1463
1464 serializeRigidBodies(serializer);
1465
1466 serializeContactManifolds(serializer);
1467
1468 serializer->finishSerialization();
1469 }
1470