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