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
17 #include "btDiscreteDynamicsWorld.h"
18
19 //collision detection
20 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
21 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
22 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
23 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
24 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
25 #include "LinearMath/btTransformUtil.h"
26 #include "LinearMath/btQuickprof.h"
27
28 //rigidbody & constraints
29 #include "BulletDynamics/Dynamics/btRigidBody.h"
30 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
31 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
32 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
33 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
34 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
35 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
36 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
37 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
38 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
39 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
40
41
42 #include "LinearMath/btIDebugDraw.h"
43 #include "BulletCollision/CollisionShapes/btSphereShape.h"
44
45
46 #include "BulletDynamics/Dynamics/btActionInterface.h"
47 #include "LinearMath/btQuickprof.h"
48 #include "LinearMath/btMotionState.h"
49
50 #include "LinearMath/btSerializer.h"
51
52 #if 0
53 btAlignedObjectArray<btVector3> debugContacts;
54 btAlignedObjectArray<btVector3> debugNormals;
55 int startHit=2;
56 int firstHit=startHit;
57 #endif
58
btGetConstraintIslandId(const btTypedConstraint * lhs)59 SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
60 {
61 int islandId;
62
63 const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
64 const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
65 islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
66 return islandId;
67
68 }
69
70
71 class btSortConstraintOnIslandPredicate
72 {
73 public:
74
operator ()(const btTypedConstraint * lhs,const btTypedConstraint * rhs) const75 bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
76 {
77 int rIslandId0,lIslandId0;
78 rIslandId0 = btGetConstraintIslandId(rhs);
79 lIslandId0 = btGetConstraintIslandId(lhs);
80 return lIslandId0 < rIslandId0;
81 }
82 };
83
84 struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
85 {
86 btContactSolverInfo* m_solverInfo;
87 btConstraintSolver* m_solver;
88 btTypedConstraint** m_sortedConstraints;
89 int m_numConstraints;
90 btIDebugDraw* m_debugDrawer;
91 btDispatcher* m_dispatcher;
92
93 btAlignedObjectArray<btCollisionObject*> m_bodies;
94 btAlignedObjectArray<btPersistentManifold*> m_manifolds;
95 btAlignedObjectArray<btTypedConstraint*> m_constraints;
96
97
InplaceSolverIslandCallbackInplaceSolverIslandCallback98 InplaceSolverIslandCallback(
99 btConstraintSolver* solver,
100 btStackAlloc* stackAlloc,
101 btDispatcher* dispatcher)
102 :m_solverInfo(NULL),
103 m_solver(solver),
104 m_sortedConstraints(NULL),
105 m_numConstraints(0),
106 m_debugDrawer(NULL),
107 m_dispatcher(dispatcher)
108 {
109
110 }
111
operator =InplaceSolverIslandCallback112 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
113 {
114 btAssert(0);
115 (void)other;
116 return *this;
117 }
118
setupInplaceSolverIslandCallback119 SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer)
120 {
121 btAssert(solverInfo);
122 m_solverInfo = solverInfo;
123 m_sortedConstraints = sortedConstraints;
124 m_numConstraints = numConstraints;
125 m_debugDrawer = debugDrawer;
126 m_bodies.resize (0);
127 m_manifolds.resize (0);
128 m_constraints.resize (0);
129 }
130
131
processIslandInplaceSolverIslandCallback132 virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
133 {
134 if (islandId<0)
135 {
136 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
137 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
138 } else
139 {
140 //also add all non-contact constraints/joints for this island
141 btTypedConstraint** startConstraint = 0;
142 int numCurConstraints = 0;
143 int i;
144
145 //find the first constraint for this island
146 for (i=0;i<m_numConstraints;i++)
147 {
148 if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
149 {
150 startConstraint = &m_sortedConstraints[i];
151 break;
152 }
153 }
154 //count the number of constraints in this island
155 for (;i<m_numConstraints;i++)
156 {
157 if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
158 {
159 numCurConstraints++;
160 }
161 }
162
163 if (m_solverInfo->m_minimumSolverBatchSize<=1)
164 {
165 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
166 } else
167 {
168
169 for (i=0;i<numBodies;i++)
170 m_bodies.push_back(bodies[i]);
171 for (i=0;i<numManifolds;i++)
172 m_manifolds.push_back(manifolds[i]);
173 for (i=0;i<numCurConstraints;i++)
174 m_constraints.push_back(startConstraint[i]);
175 if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
176 {
177 processConstraints();
178 } else
179 {
180 //printf("deferred\n");
181 }
182 }
183 }
184 }
processConstraintsInplaceSolverIslandCallback185 void processConstraints()
186 {
187
188 btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
189 btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
190 btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
191
192 m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
193 m_bodies.resize(0);
194 m_manifolds.resize(0);
195 m_constraints.resize(0);
196
197 }
198
199 };
200
201
202
btDiscreteDynamicsWorld(btDispatcher * dispatcher,btBroadphaseInterface * pairCache,btConstraintSolver * constraintSolver,btCollisionConfiguration * collisionConfiguration)203 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
204 :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
205 m_sortedConstraints (),
206 m_solverIslandCallback ( NULL ),
207 m_constraintSolver(constraintSolver),
208 m_gravity(0,-10,0),
209 m_localTime(0),
210 m_fixedTimeStep(0),
211 m_synchronizeAllMotionStates(false),
212 m_applySpeculativeContactRestitution(false),
213 m_profileTimings(0),
214 m_latencyMotionStateInterpolation(true)
215
216 {
217 if (!m_constraintSolver)
218 {
219 void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
220 m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
221 m_ownsConstraintSolver = true;
222 } else
223 {
224 m_ownsConstraintSolver = false;
225 }
226
227 {
228 void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
229 m_islandManager = new (mem) btSimulationIslandManager();
230 }
231
232 m_ownsIslandManager = true;
233
234 {
235 void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
236 m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher);
237 }
238 }
239
240
~btDiscreteDynamicsWorld()241 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
242 {
243 //only delete it when we created it
244 if (m_ownsIslandManager)
245 {
246 m_islandManager->~btSimulationIslandManager();
247 btAlignedFree( m_islandManager);
248 }
249 if (m_solverIslandCallback)
250 {
251 m_solverIslandCallback->~InplaceSolverIslandCallback();
252 btAlignedFree(m_solverIslandCallback);
253 }
254 if (m_ownsConstraintSolver)
255 {
256
257 m_constraintSolver->~btConstraintSolver();
258 btAlignedFree(m_constraintSolver);
259 }
260 }
261
saveKinematicState(btScalar timeStep)262 void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
263 {
264 ///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
265 ///to switch status _after_ adding kinematic objects to the world
266 ///fix it for Bullet 3.x release
267 for (int i=0;i<m_collisionObjects.size();i++)
268 {
269 btCollisionObject* colObj = m_collisionObjects[i];
270 btRigidBody* body = btRigidBody::upcast(colObj);
271 if (body && body->getActivationState() != ISLAND_SLEEPING)
272 {
273 if (body->isKinematicObject())
274 {
275 //to calculate velocities next frame
276 body->saveKinematicState(timeStep);
277 }
278 }
279 }
280
281 }
282
debugDrawWorld()283 void btDiscreteDynamicsWorld::debugDrawWorld()
284 {
285 BT_PROFILE("debugDrawWorld");
286
287 btCollisionWorld::debugDrawWorld();
288
289 bool drawConstraints = false;
290 if (getDebugDrawer())
291 {
292 int mode = getDebugDrawer()->getDebugMode();
293 if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
294 {
295 drawConstraints = true;
296 }
297 }
298 if(drawConstraints)
299 {
300 for(int i = getNumConstraints()-1; i>=0 ;i--)
301 {
302 btTypedConstraint* constraint = getConstraint(i);
303 debugDrawConstraint(constraint);
304 }
305 }
306
307
308
309 if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
310 {
311 int i;
312
313 if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
314 {
315 for (i=0;i<m_actions.size();i++)
316 {
317 m_actions[i]->debugDraw(m_debugDrawer);
318 }
319 }
320 }
321 if (getDebugDrawer())
322 getDebugDrawer()->flushLines();
323
324 }
325
clearForces()326 void btDiscreteDynamicsWorld::clearForces()
327 {
328 ///@todo: iterate over awake simulation islands!
329 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
330 {
331 btRigidBody* body = m_nonStaticRigidBodies[i];
332 //need to check if next line is ok
333 //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
334 body->clearForces();
335 }
336 }
337
338 ///apply gravity, call this once per timestep
applyGravity()339 void btDiscreteDynamicsWorld::applyGravity()
340 {
341 ///@todo: iterate over awake simulation islands!
342 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
343 {
344 btRigidBody* body = m_nonStaticRigidBodies[i];
345 if (body->isActive())
346 {
347 body->applyGravity();
348 }
349 }
350 }
351
352
synchronizeSingleMotionState(btRigidBody * body)353 void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
354 {
355 btAssert(body);
356
357 if (body->getMotionState() && !body->isStaticOrKinematicObject())
358 {
359 //we need to call the update at least once, even for sleeping objects
360 //otherwise the 'graphics' transform never updates properly
361 ///@todo: add 'dirty' flag
362 //if (body->getActivationState() != ISLAND_SLEEPING)
363 {
364 btTransform interpolatedTransform;
365 btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
366 body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
367 (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
368 interpolatedTransform);
369 body->getMotionState()->setWorldTransform(interpolatedTransform);
370 }
371 }
372 }
373
374
synchronizeMotionStates()375 void btDiscreteDynamicsWorld::synchronizeMotionStates()
376 {
377 BT_PROFILE("synchronizeMotionStates");
378 if (m_synchronizeAllMotionStates)
379 {
380 //iterate over all collision objects
381 for ( int i=0;i<m_collisionObjects.size();i++)
382 {
383 btCollisionObject* colObj = m_collisionObjects[i];
384 btRigidBody* body = btRigidBody::upcast(colObj);
385 if (body)
386 synchronizeSingleMotionState(body);
387 }
388 } else
389 {
390 //iterate over all active rigid bodies
391 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
392 {
393 btRigidBody* body = m_nonStaticRigidBodies[i];
394 if (body->isActive())
395 synchronizeSingleMotionState(body);
396 }
397 }
398 }
399
400
stepSimulation(btScalar timeStep,int maxSubSteps,btScalar fixedTimeStep)401 int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
402 {
403 startProfiling(timeStep);
404
405 BT_PROFILE("stepSimulation");
406
407 int numSimulationSubSteps = 0;
408
409 if (maxSubSteps)
410 {
411 //fixed timestep with interpolation
412 m_fixedTimeStep = fixedTimeStep;
413 m_localTime += timeStep;
414 if (m_localTime >= fixedTimeStep)
415 {
416 numSimulationSubSteps = int( m_localTime / fixedTimeStep);
417 m_localTime -= numSimulationSubSteps * fixedTimeStep;
418 }
419 } else
420 {
421 //variable timestep
422 fixedTimeStep = timeStep;
423 m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
424 m_fixedTimeStep = 0;
425 if (btFuzzyZero(timeStep))
426 {
427 numSimulationSubSteps = 0;
428 maxSubSteps = 0;
429 } else
430 {
431 numSimulationSubSteps = 1;
432 maxSubSteps = 1;
433 }
434 }
435
436 //process some debugging flags
437 if (getDebugDrawer())
438 {
439 btIDebugDraw* debugDrawer = getDebugDrawer ();
440 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
441 }
442 if (numSimulationSubSteps)
443 {
444
445 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
446 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
447
448 saveKinematicState(fixedTimeStep*clampedSimulationSteps);
449
450 applyGravity();
451
452
453
454 for (int i=0;i<clampedSimulationSteps;i++)
455 {
456 internalSingleStepSimulation(fixedTimeStep);
457 synchronizeMotionStates();
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,short int collisionFilterGroup,short int collisionFilterMask)542 void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short 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 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
582 short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
583
584 addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
585 }
586 }
587
addRigidBody(btRigidBody * body,short group,short mask)588 void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short 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
createPredictiveContacts(btScalar timeStep)881 void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
882 {
883 BT_PROFILE("createPredictiveContacts");
884
885 {
886 BT_PROFILE("release predictive contact manifolds");
887
888 for (int i=0;i<m_predictiveManifolds.size();i++)
889 {
890 btPersistentManifold* manifold = m_predictiveManifolds[i];
891 this->m_dispatcher1->releaseManifold(manifold);
892 }
893 m_predictiveManifolds.clear();
894 }
895
896 btTransform predictedTrans;
897 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
898 {
899 btRigidBody* body = m_nonStaticRigidBodies[i];
900 body->setHitFraction(1.f);
901
902 if (body->isActive() && (!body->isStaticOrKinematicObject()))
903 {
904
905 body->predictIntegratedTransform(timeStep, predictedTrans);
906
907 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
908
909 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
910 {
911 BT_PROFILE("predictive convexSweepTest");
912 if (body->getCollisionShape()->isConvex())
913 {
914 gNumClampedCcdMotions++;
915 #ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY
916 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
917 {
918 public:
919
920 StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
921 btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
922 {
923 }
924
925 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
926 {
927 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
928 if (!otherObj->isStaticOrKinematicObject())
929 return false;
930 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
931 }
932 };
933
934 StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
935 #else
936 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
937 #endif
938 //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
939 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
940 sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
941
942 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
943 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
944 btTransform modifiedPredictedTrans = predictedTrans;
945 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
946
947 convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
948 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
949 {
950
951 btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
952 btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
953
954
955 btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
956 m_predictiveManifolds.push_back(manifold);
957
958 btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
959 btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
960
961 btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance);
962
963 bool isPredictive = true;
964 int index = manifold->addManifoldPoint(newPoint, isPredictive);
965 btManifoldPoint& pt = manifold->getContactPoint(index);
966 pt.m_combinedRestitution = 0;
967 pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
968 pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
969 pt.m_positionWorldOnB = worldPointB;
970
971 }
972 }
973 }
974 }
975 }
976 }
integrateTransforms(btScalar timeStep)977 void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
978 {
979 BT_PROFILE("integrateTransforms");
980 btTransform predictedTrans;
981 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
982 {
983 btRigidBody* body = m_nonStaticRigidBodies[i];
984 body->setHitFraction(1.f);
985
986 if (body->isActive() && (!body->isStaticOrKinematicObject()))
987 {
988
989 body->predictIntegratedTransform(timeStep, predictedTrans);
990
991 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
992
993
994
995 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
996 {
997 BT_PROFILE("CCD motion clamping");
998 if (body->getCollisionShape()->isConvex())
999 {
1000 gNumClampedCcdMotions++;
1001 #ifdef USE_STATIC_ONLY
1002 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
1003 {
1004 public:
1005
1006 StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
1007 btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
1008 {
1009 }
1010
1011 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
1012 {
1013 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
1014 if (!otherObj->isStaticOrKinematicObject())
1015 return false;
1016 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
1017 }
1018 };
1019
1020 StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
1021 #else
1022 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
1023 #endif
1024 //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
1025 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
1026 sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
1027
1028 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
1029 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
1030 btTransform modifiedPredictedTrans = predictedTrans;
1031 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
1032
1033 convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
1034 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
1035 {
1036
1037 //printf("clamped integration to hit fraction = %f\n",fraction);
1038 body->setHitFraction(sweepResults.m_closestHitFraction);
1039 body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
1040 body->setHitFraction(0.f);
1041 body->proceedToTransform( predictedTrans);
1042
1043 #if 0
1044 btVector3 linVel = body->getLinearVelocity();
1045
1046 btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
1047 btScalar maxSpeedSqr = maxSpeed*maxSpeed;
1048 if (linVel.length2()>maxSpeedSqr)
1049 {
1050 linVel.normalize();
1051 linVel*= maxSpeed;
1052 body->setLinearVelocity(linVel);
1053 btScalar ms2 = body->getLinearVelocity().length2();
1054 body->predictIntegratedTransform(timeStep, predictedTrans);
1055
1056 btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
1057 btScalar smt = body->getCcdSquareMotionThreshold();
1058 printf("sm2=%f\n",sm2);
1059 }
1060 #else
1061
1062 //don't apply the collision response right now, it will happen next frame
1063 //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
1064 //btScalar appliedImpulse = 0.f;
1065 //btScalar depth = 0.f;
1066 //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
1067
1068
1069 #endif
1070
1071 continue;
1072 }
1073 }
1074 }
1075
1076
1077 body->proceedToTransform( predictedTrans);
1078
1079 }
1080
1081 }
1082
1083 ///this should probably be switched on by default, but it is not well tested yet
1084 if (m_applySpeculativeContactRestitution)
1085 {
1086 BT_PROFILE("apply speculative contact restitution");
1087 for (int i=0;i<m_predictiveManifolds.size();i++)
1088 {
1089 btPersistentManifold* manifold = m_predictiveManifolds[i];
1090 btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
1091 btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
1092
1093 for (int p=0;p<manifold->getNumContacts();p++)
1094 {
1095 const btManifoldPoint& pt = manifold->getContactPoint(p);
1096 btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
1097
1098 if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
1099 //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
1100 {
1101 btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
1102
1103 const btVector3& pos1 = pt.getPositionWorldOnA();
1104 const btVector3& pos2 = pt.getPositionWorldOnB();
1105
1106 btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
1107 btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
1108
1109 if (body0)
1110 body0->applyImpulse(imp,rel_pos0);
1111 if (body1)
1112 body1->applyImpulse(-imp,rel_pos1);
1113 }
1114 }
1115 }
1116 }
1117
1118 }
1119
1120
1121
1122
1123
1124
predictUnconstraintMotion(btScalar timeStep)1125 void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
1126 {
1127 BT_PROFILE("predictUnconstraintMotion");
1128 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
1129 {
1130 btRigidBody* body = m_nonStaticRigidBodies[i];
1131 if (!body->isStaticOrKinematicObject())
1132 {
1133 //don't integrate/update velocities here, it happens in the constraint solver
1134
1135 body->applyDamping(timeStep);
1136
1137 body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
1138 }
1139 }
1140 }
1141
1142
startProfiling(btScalar timeStep)1143 void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
1144 {
1145 (void)timeStep;
1146
1147 #ifndef BT_NO_PROFILE
1148 CProfileManager::Reset();
1149 #endif //BT_NO_PROFILE
1150
1151 }
1152
1153
1154
1155
1156
1157
debugDrawConstraint(btTypedConstraint * constraint)1158 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
1159 {
1160 bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
1161 bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
1162 btScalar dbgDrawSize = constraint->getDbgDrawSize();
1163 if(dbgDrawSize <= btScalar(0.f))
1164 {
1165 return;
1166 }
1167
1168 switch(constraint->getConstraintType())
1169 {
1170 case POINT2POINT_CONSTRAINT_TYPE:
1171 {
1172 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
1173 btTransform tr;
1174 tr.setIdentity();
1175 btVector3 pivot = p2pC->getPivotInA();
1176 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
1177 tr.setOrigin(pivot);
1178 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1179 // that ideally should draw the same frame
1180 pivot = p2pC->getPivotInB();
1181 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
1182 tr.setOrigin(pivot);
1183 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1184 }
1185 break;
1186 case HINGE_CONSTRAINT_TYPE:
1187 {
1188 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
1189 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
1190 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1191 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
1192 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1193 btScalar minAng = pHinge->getLowerLimit();
1194 btScalar maxAng = pHinge->getUpperLimit();
1195 if(minAng == maxAng)
1196 {
1197 break;
1198 }
1199 bool drawSect = true;
1200 if(!pHinge->hasLimit())
1201 {
1202 minAng = btScalar(0.f);
1203 maxAng = SIMD_2_PI;
1204 drawSect = false;
1205 }
1206 if(drawLimits)
1207 {
1208 btVector3& center = tr.getOrigin();
1209 btVector3 normal = tr.getBasis().getColumn(2);
1210 btVector3 axis = tr.getBasis().getColumn(0);
1211 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
1212 }
1213 }
1214 break;
1215 case CONETWIST_CONSTRAINT_TYPE:
1216 {
1217 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
1218 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1219 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1220 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1221 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1222 if(drawLimits)
1223 {
1224 //const btScalar length = btScalar(5);
1225 const btScalar length = dbgDrawSize;
1226 static int nSegments = 8*4;
1227 btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
1228 btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
1229 pPrev = tr * pPrev;
1230 for (int i=0; i<nSegments; i++)
1231 {
1232 fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
1233 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
1234 pCur = tr * pCur;
1235 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
1236
1237 if (i%(nSegments/8) == 0)
1238 getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
1239
1240 pPrev = pCur;
1241 }
1242 btScalar tws = pCT->getTwistSpan();
1243 btScalar twa = pCT->getTwistAngle();
1244 bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
1245 if(useFrameB)
1246 {
1247 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1248 }
1249 else
1250 {
1251 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1252 }
1253 btVector3 pivot = tr.getOrigin();
1254 btVector3 normal = tr.getBasis().getColumn(0);
1255 btVector3 axis1 = tr.getBasis().getColumn(1);
1256 getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
1257
1258 }
1259 }
1260 break;
1261 case D6_SPRING_CONSTRAINT_TYPE:
1262 case D6_CONSTRAINT_TYPE:
1263 {
1264 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)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 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1278 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1279 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
1280 axis = tr.getBasis().getColumn(1);
1281 btScalar ay = p6DOF->getAngle(1);
1282 btScalar az = p6DOF->getAngle(2);
1283 btScalar cy = btCos(ay);
1284 btScalar sy = btSin(ay);
1285 btScalar cz = btCos(az);
1286 btScalar sz = btSin(az);
1287 btVector3 ref;
1288 ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
1289 ref[1] = -sz*axis[0] + cz*axis[1];
1290 ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
1291 tr = p6DOF->getCalculatedTransformB();
1292 btVector3 normal = -tr.getBasis().getColumn(0);
1293 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1294 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1295 if(minFi > maxFi)
1296 {
1297 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
1298 }
1299 else if(minFi < maxFi)
1300 {
1301 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
1302 }
1303 tr = p6DOF->getCalculatedTransformA();
1304 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1305 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1306 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
1307 }
1308 }
1309 break;
1310 ///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
1311 case D6_SPRING_2_CONSTRAINT_TYPE:
1312 {
1313 {
1314 btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
1315 btTransform tr = p6DOF->getCalculatedTransformA();
1316 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1317 tr = p6DOF->getCalculatedTransformB();
1318 if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1319 if (drawLimits)
1320 {
1321 tr = p6DOF->getCalculatedTransformA();
1322 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1323 btVector3 up = tr.getBasis().getColumn(2);
1324 btVector3 axis = tr.getBasis().getColumn(0);
1325 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1326 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1327 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1328 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1329 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
1330 axis = tr.getBasis().getColumn(1);
1331 btScalar ay = p6DOF->getAngle(1);
1332 btScalar az = p6DOF->getAngle(2);
1333 btScalar cy = btCos(ay);
1334 btScalar sy = btSin(ay);
1335 btScalar cz = btCos(az);
1336 btScalar sz = btSin(az);
1337 btVector3 ref;
1338 ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
1339 ref[1] = -sz*axis[0] + cz*axis[1];
1340 ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
1341 tr = p6DOF->getCalculatedTransformB();
1342 btVector3 normal = -tr.getBasis().getColumn(0);
1343 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1344 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1345 if (minFi > maxFi)
1346 {
1347 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
1348 }
1349 else if (minFi < maxFi)
1350 {
1351 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
1352 }
1353 tr = p6DOF->getCalculatedTransformA();
1354 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1355 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1356 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
1357 }
1358 }
1359 break;
1360 }
1361 case SLIDER_CONSTRAINT_TYPE:
1362 {
1363 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
1364 btTransform tr = pSlider->getCalculatedTransformA();
1365 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1366 tr = pSlider->getCalculatedTransformB();
1367 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1368 if(drawLimits)
1369 {
1370 btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
1371 btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
1372 btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
1373 getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
1374 btVector3 normal = tr.getBasis().getColumn(0);
1375 btVector3 axis = tr.getBasis().getColumn(1);
1376 btScalar a_min = pSlider->getLowerAngLimit();
1377 btScalar a_max = pSlider->getUpperAngLimit();
1378 const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
1379 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
1380 }
1381 }
1382 break;
1383 default :
1384 break;
1385 }
1386 return;
1387 }
1388
1389
1390
1391
1392
setConstraintSolver(btConstraintSolver * solver)1393 void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
1394 {
1395 if (m_ownsConstraintSolver)
1396 {
1397 btAlignedFree( m_constraintSolver);
1398 }
1399 m_ownsConstraintSolver = false;
1400 m_constraintSolver = solver;
1401 m_solverIslandCallback->m_solver = solver;
1402 }
1403
getConstraintSolver()1404 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
1405 {
1406 return m_constraintSolver;
1407 }
1408
1409
getNumConstraints() const1410 int btDiscreteDynamicsWorld::getNumConstraints() const
1411 {
1412 return int(m_constraints.size());
1413 }
getConstraint(int index)1414 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
1415 {
1416 return m_constraints[index];
1417 }
getConstraint(int index) const1418 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
1419 {
1420 return m_constraints[index];
1421 }
1422
1423
1424
serializeRigidBodies(btSerializer * serializer)1425 void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
1426 {
1427 int i;
1428 //serialize all collision objects
1429 for (i=0;i<m_collisionObjects.size();i++)
1430 {
1431 btCollisionObject* colObj = m_collisionObjects[i];
1432 if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
1433 {
1434 int len = colObj->calculateSerializeBufferSize();
1435 btChunk* chunk = serializer->allocate(len,1);
1436 const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
1437 serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
1438 }
1439 }
1440
1441 for (i=0;i<m_constraints.size();i++)
1442 {
1443 btTypedConstraint* constraint = m_constraints[i];
1444 int size = constraint->calculateSerializeBufferSize();
1445 btChunk* chunk = serializer->allocate(size,1);
1446 const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
1447 serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
1448 }
1449 }
1450
1451
1452
1453
serializeDynamicsWorldInfo(btSerializer * serializer)1454 void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
1455 {
1456 #ifdef BT_USE_DOUBLE_PRECISION
1457 int len = sizeof(btDynamicsWorldDoubleData);
1458 btChunk* chunk = serializer->allocate(len,1);
1459 btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
1460 #else//BT_USE_DOUBLE_PRECISION
1461 int len = sizeof(btDynamicsWorldFloatData);
1462 btChunk* chunk = serializer->allocate(len,1);
1463 btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
1464 #endif//BT_USE_DOUBLE_PRECISION
1465
1466 memset(worldInfo ,0x00,len);
1467
1468 m_gravity.serialize(worldInfo->m_gravity);
1469 worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
1470 worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
1471 worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
1472 worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
1473
1474 worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
1475 worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
1476 worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
1477 worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
1478
1479 worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
1480 worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
1481 worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
1482 worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
1483
1484 worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
1485 worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
1486 worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
1487 worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
1488
1489 worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
1490 worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
1491 worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
1492 worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
1493
1494 worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
1495
1496 #ifdef BT_USE_DOUBLE_PRECISION
1497 const char* structType = "btDynamicsWorldDoubleData";
1498 #else//BT_USE_DOUBLE_PRECISION
1499 const char* structType = "btDynamicsWorldFloatData";
1500 #endif//BT_USE_DOUBLE_PRECISION
1501 serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo);
1502 }
1503
serialize(btSerializer * serializer)1504 void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
1505 {
1506
1507 serializer->startSerialization();
1508
1509 serializeDynamicsWorldInfo(serializer);
1510
1511 serializeCollisionObjects(serializer);
1512
1513 serializeRigidBodies(serializer);
1514
1515 serializer->finishSerialization();
1516 }
1517
1518