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
804 btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
805
806 if (!m_dispatcher->needsCollision(m_me, otherObj))
807 return false;
808
809 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
810 if (m_dispatcher->needsResponse(m_me, otherObj))
811 {
812 #if 0
813 ///don't do CCD when there are already contact points (touching contact/penetration)
814 btAlignedObjectArray<btPersistentManifold*> manifoldArray;
815 btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
816 if (collisionPair)
817 {
818 if (collisionPair->m_algorithm)
819 {
820 manifoldArray.resize(0);
821 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
822 for (int j=0;j<manifoldArray.size();j++)
823 {
824 btPersistentManifold* manifold = manifoldArray[j];
825 if (manifold->getNumContacts()>0)
826 return false;
827 }
828 }
829 }
830 #endif
831 return true;
832 }
833
834 return false;
835 }
836 };
837
838 ///internal debugging variable. this value shouldn't be too high
839 int gNumClampedCcdMotions = 0;
840
createPredictiveContactsInternal(btRigidBody ** bodies,int numBodies,btScalar timeStep)841 void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
842 {
843 btTransform predictedTrans;
844 for (int i = 0; i < numBodies; i++)
845 {
846 btRigidBody* body = bodies[i];
847 body->setHitFraction(1.f);
848
849 if (body->isActive() && (!body->isStaticOrKinematicObject()))
850 {
851 body->predictIntegratedTransform(timeStep, predictedTrans);
852
853 btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
854
855 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
856 {
857 BT_PROFILE("predictive convexSweepTest");
858 if (body->getCollisionShape()->isConvex())
859 {
860 gNumClampedCcdMotions++;
861 #ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY
862 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
863 {
864 public:
865 StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
866 {
867 }
868
869 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
870 {
871 btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
872 if (!otherObj->isStaticOrKinematicObject())
873 return false;
874 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
875 }
876 };
877
878 StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
879 #else
880 btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
881 #endif
882 //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
883 btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
884 sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
885
886 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
887 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
888 btTransform modifiedPredictedTrans = predictedTrans;
889 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
890
891 convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
892 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
893 {
894 btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction;
895 btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
896
897 btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
898 btMutexLock(&m_predictiveManifoldsMutex);
899 m_predictiveManifolds.push_back(manifold);
900 btMutexUnlock(&m_predictiveManifoldsMutex);
901
902 btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec;
903 btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB;
904
905 btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance);
906
907 bool isPredictive = true;
908 int index = manifold->addManifoldPoint(newPoint, isPredictive);
909 btManifoldPoint& pt = manifold->getContactPoint(index);
910 pt.m_combinedRestitution = 0;
911 pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject);
912 pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
913 pt.m_positionWorldOnB = worldPointB;
914 }
915 }
916 }
917 }
918 }
919 }
920
releasePredictiveContacts()921 void btDiscreteDynamicsWorld::releasePredictiveContacts()
922 {
923 BT_PROFILE("release predictive contact manifolds");
924
925 for (int i = 0; i < m_predictiveManifolds.size(); i++)
926 {
927 btPersistentManifold* manifold = m_predictiveManifolds[i];
928 this->m_dispatcher1->releaseManifold(manifold);
929 }
930 m_predictiveManifolds.clear();
931 }
932
createPredictiveContacts(btScalar timeStep)933 void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
934 {
935 BT_PROFILE("createPredictiveContacts");
936 releasePredictiveContacts();
937 if (m_nonStaticRigidBodies.size() > 0)
938 {
939 createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
940 }
941 }
942
integrateTransformsInternal(btRigidBody ** bodies,int numBodies,btScalar timeStep)943 void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
944 {
945 btTransform predictedTrans;
946 for (int i = 0; i < numBodies; i++)
947 {
948 btRigidBody* body = bodies[i];
949 body->setHitFraction(1.f);
950
951 if (body->isActive() && (!body->isStaticOrKinematicObject()))
952 {
953 body->predictIntegratedTransform(timeStep, predictedTrans);
954
955 btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
956
957 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
958 {
959 BT_PROFILE("CCD motion clamping");
960 if (body->getCollisionShape()->isConvex())
961 {
962 gNumClampedCcdMotions++;
963 #ifdef USE_STATIC_ONLY
964 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
965 {
966 public:
967 StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
968 {
969 }
970
971 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
972 {
973 btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
974 if (!otherObj->isStaticOrKinematicObject())
975 return false;
976 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
977 }
978 };
979
980 StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
981 #else
982 btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
983 #endif
984 //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
985 btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
986 sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
987
988 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
989 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
990 btTransform modifiedPredictedTrans = predictedTrans;
991 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
992
993 convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
994 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
995 {
996 //printf("clamped integration to hit fraction = %f\n",fraction);
997 body->setHitFraction(sweepResults.m_closestHitFraction);
998 body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans);
999 body->setHitFraction(0.f);
1000 body->proceedToTransform(predictedTrans);
1001
1002 #if 0
1003 btVector3 linVel = body->getLinearVelocity();
1004
1005 btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
1006 btScalar maxSpeedSqr = maxSpeed*maxSpeed;
1007 if (linVel.length2()>maxSpeedSqr)
1008 {
1009 linVel.normalize();
1010 linVel*= maxSpeed;
1011 body->setLinearVelocity(linVel);
1012 btScalar ms2 = body->getLinearVelocity().length2();
1013 body->predictIntegratedTransform(timeStep, predictedTrans);
1014
1015 btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
1016 btScalar smt = body->getCcdSquareMotionThreshold();
1017 printf("sm2=%f\n",sm2);
1018 }
1019 #else
1020
1021 //don't apply the collision response right now, it will happen next frame
1022 //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
1023 //btScalar appliedImpulse = 0.f;
1024 //btScalar depth = 0.f;
1025 //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
1026
1027 #endif
1028
1029 continue;
1030 }
1031 }
1032 }
1033
1034 body->proceedToTransform(predictedTrans);
1035 }
1036 }
1037 }
1038
integrateTransforms(btScalar timeStep)1039 void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
1040 {
1041 BT_PROFILE("integrateTransforms");
1042 if (m_nonStaticRigidBodies.size() > 0)
1043 {
1044 integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
1045 }
1046
1047 ///this should probably be switched on by default, but it is not well tested yet
1048 if (m_applySpeculativeContactRestitution)
1049 {
1050 BT_PROFILE("apply speculative contact restitution");
1051 for (int i = 0; i < m_predictiveManifolds.size(); i++)
1052 {
1053 btPersistentManifold* manifold = m_predictiveManifolds[i];
1054 btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
1055 btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
1056
1057 for (int p = 0; p < manifold->getNumContacts(); p++)
1058 {
1059 const btManifoldPoint& pt = manifold->getContactPoint(p);
1060 btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1);
1061
1062 if (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f)
1063 //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
1064 {
1065 btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution;
1066
1067 const btVector3& pos1 = pt.getPositionWorldOnA();
1068 const btVector3& pos2 = pt.getPositionWorldOnB();
1069
1070 btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
1071 btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
1072
1073 if (body0)
1074 body0->applyImpulse(imp, rel_pos0);
1075 if (body1)
1076 body1->applyImpulse(-imp, rel_pos1);
1077 }
1078 }
1079 }
1080 }
1081 }
1082
predictUnconstraintMotion(btScalar timeStep)1083 void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
1084 {
1085 BT_PROFILE("predictUnconstraintMotion");
1086 for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
1087 {
1088 btRigidBody* body = m_nonStaticRigidBodies[i];
1089 if (!body->isStaticOrKinematicObject())
1090 {
1091 //don't integrate/update velocities here, it happens in the constraint solver
1092
1093 body->applyDamping(timeStep);
1094
1095 body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
1096 }
1097 }
1098 }
1099
startProfiling(btScalar timeStep)1100 void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
1101 {
1102 (void)timeStep;
1103
1104 #ifndef BT_NO_PROFILE
1105 CProfileManager::Reset();
1106 #endif //BT_NO_PROFILE
1107 }
1108
debugDrawConstraint(btTypedConstraint * constraint)1109 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
1110 {
1111 bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
1112 bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
1113 btScalar dbgDrawSize = constraint->getDbgDrawSize();
1114 if (dbgDrawSize <= btScalar(0.f))
1115 {
1116 return;
1117 }
1118
1119 switch (constraint->getConstraintType())
1120 {
1121 case POINT2POINT_CONSTRAINT_TYPE:
1122 {
1123 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
1124 btTransform tr;
1125 tr.setIdentity();
1126 btVector3 pivot = p2pC->getPivotInA();
1127 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
1128 tr.setOrigin(pivot);
1129 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1130 // that ideally should draw the same frame
1131 pivot = p2pC->getPivotInB();
1132 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
1133 tr.setOrigin(pivot);
1134 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1135 }
1136 break;
1137 case HINGE_CONSTRAINT_TYPE:
1138 {
1139 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
1140 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
1141 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1142 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
1143 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1144 btScalar minAng = pHinge->getLowerLimit();
1145 btScalar maxAng = pHinge->getUpperLimit();
1146 if (minAng == maxAng)
1147 {
1148 break;
1149 }
1150 bool drawSect = true;
1151 if (!pHinge->hasLimit())
1152 {
1153 minAng = btScalar(0.f);
1154 maxAng = SIMD_2_PI;
1155 drawSect = false;
1156 }
1157 if (drawLimits)
1158 {
1159 btVector3& center = tr.getOrigin();
1160 btVector3 normal = tr.getBasis().getColumn(2);
1161 btVector3 axis = tr.getBasis().getColumn(0);
1162 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect);
1163 }
1164 }
1165 break;
1166 case CONETWIST_CONSTRAINT_TYPE:
1167 {
1168 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
1169 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1170 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1171 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1172 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1173 if (drawLimits)
1174 {
1175 //const btScalar length = btScalar(5);
1176 const btScalar length = dbgDrawSize;
1177 static int nSegments = 8 * 4;
1178 btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments);
1179 btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
1180 pPrev = tr * pPrev;
1181 for (int i = 0; i < nSegments; i++)
1182 {
1183 fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments);
1184 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
1185 pCur = tr * pCur;
1186 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0));
1187
1188 if (i % (nSegments / 8) == 0)
1189 getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0, 0, 0));
1190
1191 pPrev = pCur;
1192 }
1193 btScalar tws = pCT->getTwistSpan();
1194 btScalar twa = pCT->getTwistAngle();
1195 bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
1196 if (useFrameB)
1197 {
1198 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1199 }
1200 else
1201 {
1202 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1203 }
1204 btVector3 pivot = tr.getOrigin();
1205 btVector3 normal = tr.getBasis().getColumn(0);
1206 btVector3 axis1 = tr.getBasis().getColumn(1);
1207 getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true);
1208 }
1209 }
1210 break;
1211 case D6_SPRING_CONSTRAINT_TYPE:
1212 case D6_CONSTRAINT_TYPE:
1213 {
1214 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
1215 btTransform tr = p6DOF->getCalculatedTransformA();
1216 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1217 tr = p6DOF->getCalculatedTransformB();
1218 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1219 if (drawLimits)
1220 {
1221 tr = p6DOF->getCalculatedTransformA();
1222 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1223 btVector3 up = tr.getBasis().getColumn(2);
1224 btVector3 axis = tr.getBasis().getColumn(0);
1225 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1226 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1227 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1228 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1229 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
1230 axis = tr.getBasis().getColumn(1);
1231 btScalar ay = p6DOF->getAngle(1);
1232 btScalar az = p6DOF->getAngle(2);
1233 btScalar cy = btCos(ay);
1234 btScalar sy = btSin(ay);
1235 btScalar cz = btCos(az);
1236 btScalar sz = btSin(az);
1237 btVector3 ref;
1238 ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
1239 ref[1] = -sz * axis[0] + cz * axis[1];
1240 ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
1241 tr = p6DOF->getCalculatedTransformB();
1242 btVector3 normal = -tr.getBasis().getColumn(0);
1243 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1244 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1245 if (minFi > maxFi)
1246 {
1247 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
1248 }
1249 else if (minFi < maxFi)
1250 {
1251 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
1252 }
1253 tr = p6DOF->getCalculatedTransformA();
1254 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1255 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1256 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
1257 }
1258 }
1259 break;
1260 ///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
1261 case D6_SPRING_2_CONSTRAINT_TYPE:
1262 {
1263 {
1264 btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
1265 btTransform tr = p6DOF->getCalculatedTransformA();
1266 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1267 tr = p6DOF->getCalculatedTransformB();
1268 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1269 if (drawLimits)
1270 {
1271 tr = p6DOF->getCalculatedTransformA();
1272 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1273 btVector3 up = tr.getBasis().getColumn(2);
1274 btVector3 axis = tr.getBasis().getColumn(0);
1275 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1276 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1277 if (minTh <= maxTh)
1278 {
1279 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1280 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1281 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
1282 }
1283 axis = tr.getBasis().getColumn(1);
1284 btScalar ay = p6DOF->getAngle(1);
1285 btScalar az = p6DOF->getAngle(2);
1286 btScalar cy = btCos(ay);
1287 btScalar sy = btSin(ay);
1288 btScalar cz = btCos(az);
1289 btScalar sz = btSin(az);
1290 btVector3 ref;
1291 ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
1292 ref[1] = -sz * axis[0] + cz * axis[1];
1293 ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
1294 tr = p6DOF->getCalculatedTransformB();
1295 btVector3 normal = -tr.getBasis().getColumn(0);
1296 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1297 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1298 if (minFi > maxFi)
1299 {
1300 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
1301 }
1302 else if (minFi < maxFi)
1303 {
1304 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
1305 }
1306 tr = p6DOF->getCalculatedTransformA();
1307 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1308 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1309 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
1310 }
1311 }
1312 break;
1313 }
1314 case SLIDER_CONSTRAINT_TYPE:
1315 {
1316 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
1317 btTransform tr = pSlider->getCalculatedTransformA();
1318 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1319 tr = pSlider->getCalculatedTransformB();
1320 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1321 if (drawLimits)
1322 {
1323 btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
1324 btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
1325 btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
1326 getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
1327 btVector3 normal = tr.getBasis().getColumn(0);
1328 btVector3 axis = tr.getBasis().getColumn(1);
1329 btScalar a_min = pSlider->getLowerAngLimit();
1330 btScalar a_max = pSlider->getUpperAngLimit();
1331 const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
1332 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true);
1333 }
1334 }
1335 break;
1336 default:
1337 break;
1338 }
1339 return;
1340 }
1341
setConstraintSolver(btConstraintSolver * solver)1342 void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
1343 {
1344 if (m_ownsConstraintSolver)
1345 {
1346 btAlignedFree(m_constraintSolver);
1347 }
1348 m_ownsConstraintSolver = false;
1349 m_constraintSolver = solver;
1350 m_solverIslandCallback->m_solver = solver;
1351 }
1352
getConstraintSolver()1353 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
1354 {
1355 return m_constraintSolver;
1356 }
1357
getNumConstraints() const1358 int btDiscreteDynamicsWorld::getNumConstraints() const
1359 {
1360 return int(m_constraints.size());
1361 }
getConstraint(int index)1362 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
1363 {
1364 return m_constraints[index];
1365 }
getConstraint(int index) const1366 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
1367 {
1368 return m_constraints[index];
1369 }
1370
serializeRigidBodies(btSerializer * serializer)1371 void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
1372 {
1373 int i;
1374 //serialize all collision objects
1375 for (i = 0; i < m_collisionObjects.size(); i++)
1376 {
1377 btCollisionObject* colObj = m_collisionObjects[i];
1378 if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
1379 {
1380 int len = colObj->calculateSerializeBufferSize();
1381 btChunk* chunk = serializer->allocate(len, 1);
1382 const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
1383 serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj);
1384 }
1385 }
1386
1387 for (i = 0; i < m_constraints.size(); i++)
1388 {
1389 btTypedConstraint* constraint = m_constraints[i];
1390 int size = constraint->calculateSerializeBufferSize();
1391 btChunk* chunk = serializer->allocate(size, 1);
1392 const char* structType = constraint->serialize(chunk->m_oldPtr, serializer);
1393 serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint);
1394 }
1395 }
1396
serializeDynamicsWorldInfo(btSerializer * serializer)1397 void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
1398 {
1399 #ifdef BT_USE_DOUBLE_PRECISION
1400 int len = sizeof(btDynamicsWorldDoubleData);
1401 btChunk* chunk = serializer->allocate(len, 1);
1402 btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
1403 #else //BT_USE_DOUBLE_PRECISION
1404 int len = sizeof(btDynamicsWorldFloatData);
1405 btChunk* chunk = serializer->allocate(len, 1);
1406 btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
1407 #endif //BT_USE_DOUBLE_PRECISION
1408
1409 memset(worldInfo, 0x00, len);
1410
1411 m_gravity.serialize(worldInfo->m_gravity);
1412 worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
1413 worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
1414 worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
1415 worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
1416
1417 worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
1418 worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
1419 worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
1420 worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
1421
1422 worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
1423 worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
1424 worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
1425 worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
1426
1427 worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
1428 worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
1429 worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
1430 worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
1431
1432 worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
1433 worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
1434 worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
1435 worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
1436
1437 worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
1438
1439
1440 #ifdef BT_USE_DOUBLE_PRECISION
1441 const char* structType = "btDynamicsWorldDoubleData";
1442 #else //BT_USE_DOUBLE_PRECISION
1443 const char* structType = "btDynamicsWorldFloatData";
1444 #endif //BT_USE_DOUBLE_PRECISION
1445 serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo);
1446 }
1447
serialize(btSerializer * serializer)1448 void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
1449 {
1450 serializer->startSerialization();
1451
1452 serializeDynamicsWorldInfo(serializer);
1453
1454 serializeCollisionObjects(serializer);
1455
1456 serializeRigidBodies(serializer);
1457
1458 serializeContactManifolds(serializer);
1459
1460 serializer->finishSerialization();
1461 }
1462