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