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