00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btDiscreteDynamicsWorld.h"
00018
00019
00020 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00021 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
00022 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
00023 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
00024 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
00025 #include "LinearMath/btTransformUtil.h"
00026 #include "LinearMath/btQuickprof.h"
00027
00028
00029 #include "BulletDynamics/Dynamics/btRigidBody.h"
00030 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
00031 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
00032 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00033 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
00034 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
00035 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
00036 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
00037 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
00038 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
00039
00040
00041 #include "LinearMath/btIDebugDraw.h"
00042 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00043
00044
00045 #include "BulletDynamics/Dynamics/btActionInterface.h"
00046 #include "LinearMath/btQuickprof.h"
00047 #include "LinearMath/btMotionState.h"
00048
00049 #include "LinearMath/btSerializer.h"
00050
00051 #if 0
00052 btAlignedObjectArray<btVector3> debugContacts;
00053 btAlignedObjectArray<btVector3> debugNormals;
00054 int startHit=2;
00055 int firstHit=startHit;
00056 #endif
00057
00058 SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
00059 {
00060 int islandId;
00061
00062 const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
00063 const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
00064 islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
00065 return islandId;
00066
00067 }
00068
00069
00070 class btSortConstraintOnIslandPredicate
00071 {
00072 public:
00073
00074 bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
00075 {
00076 int rIslandId0,lIslandId0;
00077 rIslandId0 = btGetConstraintIslandId(rhs);
00078 lIslandId0 = btGetConstraintIslandId(lhs);
00079 return lIslandId0 < rIslandId0;
00080 }
00081 };
00082
00083 struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
00084 {
00085 btContactSolverInfo* m_solverInfo;
00086 btConstraintSolver* m_solver;
00087 btTypedConstraint** m_sortedConstraints;
00088 int m_numConstraints;
00089 btIDebugDraw* m_debugDrawer;
00090 btStackAlloc* m_stackAlloc;
00091 btDispatcher* m_dispatcher;
00092
00093 btAlignedObjectArray<btCollisionObject*> m_bodies;
00094 btAlignedObjectArray<btPersistentManifold*> m_manifolds;
00095 btAlignedObjectArray<btTypedConstraint*> m_constraints;
00096
00097
00098 InplaceSolverIslandCallback(
00099 btConstraintSolver* solver,
00100 btStackAlloc* stackAlloc,
00101 btDispatcher* dispatcher)
00102 :m_solverInfo(NULL),
00103 m_solver(solver),
00104 m_sortedConstraints(NULL),
00105 m_numConstraints(0),
00106 m_debugDrawer(NULL),
00107 m_stackAlloc(stackAlloc),
00108 m_dispatcher(dispatcher)
00109 {
00110
00111 }
00112
00113 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
00114 {
00115 btAssert(0);
00116 (void)other;
00117 return *this;
00118 }
00119
00120 SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer)
00121 {
00122 btAssert(solverInfo);
00123 m_solverInfo = solverInfo;
00124 m_sortedConstraints = sortedConstraints;
00125 m_numConstraints = numConstraints;
00126 m_debugDrawer = debugDrawer;
00127 m_bodies.resize (0);
00128 m_manifolds.resize (0);
00129 m_constraints.resize (0);
00130 }
00131
00132
00133 virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
00134 {
00135 if (islandId<0)
00136 {
00138 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
00139 } else
00140 {
00141
00142 btTypedConstraint** startConstraint = 0;
00143 int numCurConstraints = 0;
00144 int i;
00145
00146
00147 for (i=0;i<m_numConstraints;i++)
00148 {
00149 if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
00150 {
00151 startConstraint = &m_sortedConstraints[i];
00152 break;
00153 }
00154 }
00155
00156 for (;i<m_numConstraints;i++)
00157 {
00158 if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
00159 {
00160 numCurConstraints++;
00161 }
00162 }
00163
00164 if (m_solverInfo->m_minimumSolverBatchSize<=1)
00165 {
00166 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
00167 } else
00168 {
00169
00170 for (i=0;i<numBodies;i++)
00171 m_bodies.push_back(bodies[i]);
00172 for (i=0;i<numManifolds;i++)
00173 m_manifolds.push_back(manifolds[i]);
00174 for (i=0;i<numCurConstraints;i++)
00175 m_constraints.push_back(startConstraint[i]);
00176 if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
00177 {
00178 processConstraints();
00179 } else
00180 {
00181
00182 }
00183 }
00184 }
00185 }
00186 void processConstraints()
00187 {
00188
00189 btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
00190 btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
00191 btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
00192
00193 m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
00194 m_bodies.resize(0);
00195 m_manifolds.resize(0);
00196 m_constraints.resize(0);
00197
00198 }
00199
00200 };
00201
00202
00203
00204 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
00205 :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
00206 m_sortedConstraints (),
00207 m_solverIslandCallback ( NULL ),
00208 m_constraintSolver(constraintSolver),
00209 m_gravity(0,-10,0),
00210 m_localTime(0),
00211 m_synchronizeAllMotionStates(false),
00212 m_applySpeculativeContactRestitution(false),
00213 m_profileTimings(0)
00214
00215 {
00216 if (!m_constraintSolver)
00217 {
00218 void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
00219 m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
00220 m_ownsConstraintSolver = true;
00221 } else
00222 {
00223 m_ownsConstraintSolver = false;
00224 }
00225
00226 {
00227 void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
00228 m_islandManager = new (mem) btSimulationIslandManager();
00229 }
00230
00231 m_ownsIslandManager = true;
00232
00233 {
00234 void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
00235 m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, m_stackAlloc, dispatcher);
00236 }
00237 }
00238
00239
00240 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
00241 {
00242
00243 if (m_ownsIslandManager)
00244 {
00245 m_islandManager->~btSimulationIslandManager();
00246 btAlignedFree( m_islandManager);
00247 }
00248 if (m_solverIslandCallback)
00249 {
00250 m_solverIslandCallback->~InplaceSolverIslandCallback();
00251 btAlignedFree(m_solverIslandCallback);
00252 }
00253 if (m_ownsConstraintSolver)
00254 {
00255
00256 m_constraintSolver->~btConstraintSolver();
00257 btAlignedFree(m_constraintSolver);
00258 }
00259 }
00260
00261 void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
00262 {
00266 for (int i=0;i<m_collisionObjects.size();i++)
00267 {
00268 btCollisionObject* colObj = m_collisionObjects[i];
00269 btRigidBody* body = btRigidBody::upcast(colObj);
00270 if (body && body->getActivationState() != ISLAND_SLEEPING)
00271 {
00272 if (body->isKinematicObject())
00273 {
00274
00275 body->saveKinematicState(timeStep);
00276 }
00277 }
00278 }
00279
00280 }
00281
00282 void btDiscreteDynamicsWorld::debugDrawWorld()
00283 {
00284 BT_PROFILE("debugDrawWorld");
00285
00286 btCollisionWorld::debugDrawWorld();
00287
00288 bool drawConstraints = false;
00289 if (getDebugDrawer())
00290 {
00291 int mode = getDebugDrawer()->getDebugMode();
00292 if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
00293 {
00294 drawConstraints = true;
00295 }
00296 }
00297 if(drawConstraints)
00298 {
00299 for(int i = getNumConstraints()-1; i>=0 ;i--)
00300 {
00301 btTypedConstraint* constraint = getConstraint(i);
00302 debugDrawConstraint(constraint);
00303 }
00304 }
00305
00306
00307
00308 if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
00309 {
00310 int i;
00311
00312 if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
00313 {
00314 for (i=0;i<m_actions.size();i++)
00315 {
00316 m_actions[i]->debugDraw(m_debugDrawer);
00317 }
00318 }
00319 }
00320 }
00321
00322 void btDiscreteDynamicsWorld::clearForces()
00323 {
00325 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00326 {
00327 btRigidBody* body = m_nonStaticRigidBodies[i];
00328
00329
00330 body->clearForces();
00331 }
00332 }
00333
00335 void btDiscreteDynamicsWorld::applyGravity()
00336 {
00338 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00339 {
00340 btRigidBody* body = m_nonStaticRigidBodies[i];
00341 if (body->isActive())
00342 {
00343 body->applyGravity();
00344 }
00345 }
00346 }
00347
00348
00349 void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
00350 {
00351 btAssert(body);
00352
00353 if (body->getMotionState() && !body->isStaticOrKinematicObject())
00354 {
00355
00356
00358
00359 {
00360 btTransform interpolatedTransform;
00361 btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
00362 body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
00363 body->getMotionState()->setWorldTransform(interpolatedTransform);
00364 }
00365 }
00366 }
00367
00368
00369 void btDiscreteDynamicsWorld::synchronizeMotionStates()
00370 {
00371 BT_PROFILE("synchronizeMotionStates");
00372 if (m_synchronizeAllMotionStates)
00373 {
00374
00375 for ( int i=0;i<m_collisionObjects.size();i++)
00376 {
00377 btCollisionObject* colObj = m_collisionObjects[i];
00378 btRigidBody* body = btRigidBody::upcast(colObj);
00379 if (body)
00380 synchronizeSingleMotionState(body);
00381 }
00382 } else
00383 {
00384
00385 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00386 {
00387 btRigidBody* body = m_nonStaticRigidBodies[i];
00388 if (body->isActive())
00389 synchronizeSingleMotionState(body);
00390 }
00391 }
00392 }
00393
00394
00395 int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
00396 {
00397 startProfiling(timeStep);
00398
00399 BT_PROFILE("stepSimulation");
00400
00401 int numSimulationSubSteps = 0;
00402
00403 if (maxSubSteps)
00404 {
00405
00406 m_localTime += timeStep;
00407 if (m_localTime >= fixedTimeStep)
00408 {
00409 numSimulationSubSteps = int( m_localTime / fixedTimeStep);
00410 m_localTime -= numSimulationSubSteps * fixedTimeStep;
00411 }
00412 } else
00413 {
00414
00415 fixedTimeStep = timeStep;
00416 m_localTime = timeStep;
00417 if (btFuzzyZero(timeStep))
00418 {
00419 numSimulationSubSteps = 0;
00420 maxSubSteps = 0;
00421 } else
00422 {
00423 numSimulationSubSteps = 1;
00424 maxSubSteps = 1;
00425 }
00426 }
00427
00428
00429 if (getDebugDrawer())
00430 {
00431 btIDebugDraw* debugDrawer = getDebugDrawer ();
00432 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
00433 }
00434 if (numSimulationSubSteps)
00435 {
00436
00437
00438 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
00439
00440 saveKinematicState(fixedTimeStep*clampedSimulationSteps);
00441
00442 applyGravity();
00443
00444
00445
00446 for (int i=0;i<clampedSimulationSteps;i++)
00447 {
00448 internalSingleStepSimulation(fixedTimeStep);
00449 synchronizeMotionStates();
00450 }
00451
00452 } else
00453 {
00454 synchronizeMotionStates();
00455 }
00456
00457 clearForces();
00458
00459 #ifndef BT_NO_PROFILE
00460 CProfileManager::Increment_Frame_Counter();
00461 #endif //BT_NO_PROFILE
00462
00463 return numSimulationSubSteps;
00464 }
00465
00466 void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
00467 {
00468
00469 BT_PROFILE("internalSingleStepSimulation");
00470
00471 if(0 != m_internalPreTickCallback) {
00472 (*m_internalPreTickCallback)(this, timeStep);
00473 }
00474
00476 predictUnconstraintMotion(timeStep);
00477
00478 btDispatcherInfo& dispatchInfo = getDispatchInfo();
00479
00480 dispatchInfo.m_timeStep = timeStep;
00481 dispatchInfo.m_stepCount = 0;
00482 dispatchInfo.m_debugDraw = getDebugDrawer();
00483
00484
00485 createPredictiveContacts(timeStep);
00486
00488 performDiscreteCollisionDetection();
00489
00490 calculateSimulationIslands();
00491
00492
00493 getSolverInfo().m_timeStep = timeStep;
00494
00495
00496
00498 solveConstraints(getSolverInfo());
00499
00501
00503
00504 integrateTransforms(timeStep);
00505
00507 updateActions(timeStep);
00508
00509 updateActivationState( timeStep );
00510
00511 if(0 != m_internalTickCallback) {
00512 (*m_internalTickCallback)(this, timeStep);
00513 }
00514 }
00515
00516 void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
00517 {
00518 m_gravity = gravity;
00519 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00520 {
00521 btRigidBody* body = m_nonStaticRigidBodies[i];
00522 if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
00523 {
00524 body->setGravity(gravity);
00525 }
00526 }
00527 }
00528
00529 btVector3 btDiscreteDynamicsWorld::getGravity () const
00530 {
00531 return m_gravity;
00532 }
00533
00534 void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
00535 {
00536 btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
00537 }
00538
00539 void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
00540 {
00541 btRigidBody* body = btRigidBody::upcast(collisionObject);
00542 if (body)
00543 removeRigidBody(body);
00544 else
00545 btCollisionWorld::removeCollisionObject(collisionObject);
00546 }
00547
00548 void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
00549 {
00550 m_nonStaticRigidBodies.remove(body);
00551 btCollisionWorld::removeCollisionObject(body);
00552 }
00553
00554
00555 void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
00556 {
00557 if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
00558 {
00559 body->setGravity(m_gravity);
00560 }
00561
00562 if (body->getCollisionShape())
00563 {
00564 if (!body->isStaticObject())
00565 {
00566 m_nonStaticRigidBodies.push_back(body);
00567 } else
00568 {
00569 body->setActivationState(ISLAND_SLEEPING);
00570 }
00571
00572 bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
00573 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
00574 short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
00575
00576 addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
00577 }
00578 }
00579
00580 void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
00581 {
00582 if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
00583 {
00584 body->setGravity(m_gravity);
00585 }
00586
00587 if (body->getCollisionShape())
00588 {
00589 if (!body->isStaticObject())
00590 {
00591 m_nonStaticRigidBodies.push_back(body);
00592 }
00593 else
00594 {
00595 body->setActivationState(ISLAND_SLEEPING);
00596 }
00597 addCollisionObject(body,group,mask);
00598 }
00599 }
00600
00601
00602 void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
00603 {
00604 BT_PROFILE("updateActions");
00605
00606 for ( int i=0;i<m_actions.size();i++)
00607 {
00608 m_actions[i]->updateAction( this, timeStep);
00609 }
00610 }
00611
00612
00613 void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
00614 {
00615 BT_PROFILE("updateActivationState");
00616
00617 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00618 {
00619 btRigidBody* body = m_nonStaticRigidBodies[i];
00620 if (body)
00621 {
00622 body->updateDeactivation(timeStep);
00623
00624 if (body->wantsSleeping())
00625 {
00626 if (body->isStaticOrKinematicObject())
00627 {
00628 body->setActivationState(ISLAND_SLEEPING);
00629 } else
00630 {
00631 if (body->getActivationState() == ACTIVE_TAG)
00632 body->setActivationState( WANTS_DEACTIVATION );
00633 if (body->getActivationState() == ISLAND_SLEEPING)
00634 {
00635 body->setAngularVelocity(btVector3(0,0,0));
00636 body->setLinearVelocity(btVector3(0,0,0));
00637 }
00638
00639 }
00640 } else
00641 {
00642 if (body->getActivationState() != DISABLE_DEACTIVATION)
00643 body->setActivationState( ACTIVE_TAG );
00644 }
00645 }
00646 }
00647 }
00648
00649 void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
00650 {
00651 m_constraints.push_back(constraint);
00652 if (disableCollisionsBetweenLinkedBodies)
00653 {
00654 constraint->getRigidBodyA().addConstraintRef(constraint);
00655 constraint->getRigidBodyB().addConstraintRef(constraint);
00656 }
00657 }
00658
00659 void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
00660 {
00661 m_constraints.remove(constraint);
00662 constraint->getRigidBodyA().removeConstraintRef(constraint);
00663 constraint->getRigidBodyB().removeConstraintRef(constraint);
00664 }
00665
00666 void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
00667 {
00668 m_actions.push_back(action);
00669 }
00670
00671 void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
00672 {
00673 m_actions.remove(action);
00674 }
00675
00676
00677 void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
00678 {
00679 addAction(vehicle);
00680 }
00681
00682 void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
00683 {
00684 removeAction(vehicle);
00685 }
00686
00687 void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
00688 {
00689 addAction(character);
00690 }
00691
00692 void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
00693 {
00694 removeAction(character);
00695 }
00696
00697
00698
00699
00700 void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
00701 {
00702 BT_PROFILE("solveConstraints");
00703
00704 m_sortedConstraints.resize( m_constraints.size());
00705 int i;
00706 for (i=0;i<getNumConstraints();i++)
00707 {
00708 m_sortedConstraints[i] = m_constraints[i];
00709 }
00710
00711
00712
00713
00714
00715 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
00716
00717 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
00718
00719 m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
00720 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
00721
00723 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
00724
00725 m_solverIslandCallback->processConstraints();
00726
00727 m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
00728 }
00729
00730
00731 void btDiscreteDynamicsWorld::calculateSimulationIslands()
00732 {
00733 BT_PROFILE("calculateSimulationIslands");
00734
00735 getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
00736
00737 {
00738
00739 for (int i=0;i<this->m_predictiveManifolds.size();i++)
00740 {
00741 btPersistentManifold* manifold = m_predictiveManifolds[i];
00742
00743 const btCollisionObject* colObj0 = manifold->getBody0();
00744 const btCollisionObject* colObj1 = manifold->getBody1();
00745
00746 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
00747 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
00748 {
00749 if (colObj0->isActive() || colObj1->isActive())
00750 {
00751
00752 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
00753 (colObj1)->getIslandTag());
00754 }
00755 }
00756 }
00757 }
00758
00759 {
00760 int i;
00761 int numConstraints = int(m_constraints.size());
00762 for (i=0;i< numConstraints ; i++ )
00763 {
00764 btTypedConstraint* constraint = m_constraints[i];
00765 if (constraint->isEnabled())
00766 {
00767 const btRigidBody* colObj0 = &constraint->getRigidBodyA();
00768 const btRigidBody* colObj1 = &constraint->getRigidBodyB();
00769
00770 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
00771 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
00772 {
00773 if (colObj0->isActive() || colObj1->isActive())
00774 {
00775
00776 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
00777 (colObj1)->getIslandTag());
00778 }
00779 }
00780 }
00781 }
00782 }
00783
00784
00785 getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
00786
00787
00788 }
00789
00790
00791
00792
00793 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
00794 {
00795 public:
00796
00797 btCollisionObject* m_me;
00798 btScalar m_allowedPenetration;
00799 btOverlappingPairCache* m_pairCache;
00800 btDispatcher* m_dispatcher;
00801
00802 public:
00803 btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
00804 btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
00805 m_me(me),
00806 m_allowedPenetration(0.0f),
00807 m_pairCache(pairCache),
00808 m_dispatcher(dispatcher)
00809 {
00810 }
00811
00812 virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
00813 {
00814 if (convexResult.m_hitCollisionObject == m_me)
00815 return 1.0f;
00816
00817
00818 if(!convexResult.m_hitCollisionObject->hasContactResponse())
00819 return 1.0f;
00820
00821 btVector3 linVelA,linVelB;
00822 linVelA = m_convexToWorld-m_convexFromWorld;
00823 linVelB = btVector3(0,0,0);
00824
00825 btVector3 relativeVelocity = (linVelA-linVelB);
00826
00827 if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
00828 return 1.f;
00829
00830 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
00831 }
00832
00833 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
00834 {
00835
00836 if (proxy0->m_clientObject == m_me)
00837 return false;
00838
00840 if (!ClosestConvexResultCallback::needsCollision(proxy0))
00841 return false;
00842
00843 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
00844
00845
00846 if (m_dispatcher->needsResponse(m_me,otherObj))
00847 {
00848 #if 0
00849
00850 btAlignedObjectArray<btPersistentManifold*> manifoldArray;
00851 btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
00852 if (collisionPair)
00853 {
00854 if (collisionPair->m_algorithm)
00855 {
00856 manifoldArray.resize(0);
00857 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
00858 for (int j=0;j<manifoldArray.size();j++)
00859 {
00860 btPersistentManifold* manifold = manifoldArray[j];
00861 if (manifold->getNumContacts()>0)
00862 return false;
00863 }
00864 }
00865 }
00866 #endif
00867 return true;
00868 }
00869
00870 return false;
00871 }
00872
00873
00874 };
00875
00877 int gNumClampedCcdMotions=0;
00878
00879
00880 void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
00881 {
00882 BT_PROFILE("createPredictiveContacts");
00883
00884 {
00885 BT_PROFILE("release predictive contact manifolds");
00886
00887 for (int i=0;i<m_predictiveManifolds.size();i++)
00888 {
00889 btPersistentManifold* manifold = m_predictiveManifolds[i];
00890 this->m_dispatcher1->releaseManifold(manifold);
00891 }
00892 m_predictiveManifolds.clear();
00893 }
00894
00895 btTransform predictedTrans;
00896 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00897 {
00898 btRigidBody* body = m_nonStaticRigidBodies[i];
00899 body->setHitFraction(1.f);
00900
00901 if (body->isActive() && (!body->isStaticOrKinematicObject()))
00902 {
00903
00904 body->predictIntegratedTransform(timeStep, predictedTrans);
00905
00906 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
00907
00908 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
00909 {
00910 BT_PROFILE("predictive convexSweepTest");
00911 if (body->getCollisionShape()->isConvex())
00912 {
00913 gNumClampedCcdMotions++;
00914 #ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY
00915 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
00916 {
00917 public:
00918
00919 StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
00920 btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
00921 {
00922 }
00923
00924 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
00925 {
00926 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
00927 if (!otherObj->isStaticOrKinematicObject())
00928 return false;
00929 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
00930 }
00931 };
00932
00933 StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
00934 #else
00935 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
00936 #endif
00937
00938 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());
00939 sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
00940
00941 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
00942 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
00943 btTransform modifiedPredictedTrans = predictedTrans;
00944 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
00945
00946 convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
00947 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
00948 {
00949
00950 btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
00951 btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
00952
00953
00954 btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
00955 m_predictiveManifolds.push_back(manifold);
00956
00957 btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
00958 btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
00959
00960 btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance);
00961
00962 bool isPredictive = true;
00963 int index = manifold->addManifoldPoint(newPoint, isPredictive);
00964 btManifoldPoint& pt = manifold->getContactPoint(index);
00965 pt.m_combinedRestitution = 0;
00966 pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
00967 pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
00968 pt.m_positionWorldOnB = worldPointB;
00969
00970 }
00971 }
00972 }
00973 }
00974 }
00975 }
00976 void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
00977 {
00978 BT_PROFILE("integrateTransforms");
00979 btTransform predictedTrans;
00980 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
00981 {
00982 btRigidBody* body = m_nonStaticRigidBodies[i];
00983 body->setHitFraction(1.f);
00984
00985 if (body->isActive() && (!body->isStaticOrKinematicObject()))
00986 {
00987
00988 body->predictIntegratedTransform(timeStep, predictedTrans);
00989
00990 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
00991
00992
00993
00994 if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
00995 {
00996 BT_PROFILE("CCD motion clamping");
00997 if (body->getCollisionShape()->isConvex())
00998 {
00999 gNumClampedCcdMotions++;
01000 #ifdef USE_STATIC_ONLY
01001 class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
01002 {
01003 public:
01004
01005 StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
01006 btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
01007 {
01008 }
01009
01010 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
01011 {
01012 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
01013 if (!otherObj->isStaticOrKinematicObject())
01014 return false;
01015 return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
01016 }
01017 };
01018
01019 StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
01020 #else
01021 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
01022 #endif
01023
01024 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());
01025 sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
01026
01027 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
01028 sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
01029 btTransform modifiedPredictedTrans = predictedTrans;
01030 modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
01031
01032 convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
01033 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
01034 {
01035
01036
01037 body->setHitFraction(sweepResults.m_closestHitFraction);
01038 body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
01039 body->setHitFraction(0.f);
01040 body->proceedToTransform( predictedTrans);
01041
01042 #if 0
01043 btVector3 linVel = body->getLinearVelocity();
01044
01045 btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
01046 btScalar maxSpeedSqr = maxSpeed*maxSpeed;
01047 if (linVel.length2()>maxSpeedSqr)
01048 {
01049 linVel.normalize();
01050 linVel*= maxSpeed;
01051 body->setLinearVelocity(linVel);
01052 btScalar ms2 = body->getLinearVelocity().length2();
01053 body->predictIntegratedTransform(timeStep, predictedTrans);
01054
01055 btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
01056 btScalar smt = body->getCcdSquareMotionThreshold();
01057 printf("sm2=%f\n",sm2);
01058 }
01059 #else
01060
01061
01062
01063
01064
01065
01066
01067
01068 #endif
01069
01070 continue;
01071 }
01072 }
01073 }
01074
01075
01076 body->proceedToTransform( predictedTrans);
01077
01078 }
01079
01080 }
01081
01083 if (m_applySpeculativeContactRestitution)
01084 {
01085 BT_PROFILE("apply speculative contact restitution");
01086 for (int i=0;i<m_predictiveManifolds.size();i++)
01087 {
01088 btPersistentManifold* manifold = m_predictiveManifolds[i];
01089 btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
01090 btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
01091
01092 for (int p=0;p<manifold->getNumContacts();p++)
01093 {
01094 const btManifoldPoint& pt = manifold->getContactPoint(p);
01095 btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
01096
01097 if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
01098
01099 {
01100 btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
01101
01102 const btVector3& pos1 = pt.getPositionWorldOnA();
01103 const btVector3& pos2 = pt.getPositionWorldOnB();
01104
01105 btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
01106 btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
01107
01108 if (body0)
01109 body0->applyImpulse(imp,rel_pos0);
01110 if (body1)
01111 body1->applyImpulse(-imp,rel_pos1);
01112 }
01113 }
01114 }
01115 }
01116
01117 }
01118
01119
01120
01121
01122
01123
01124 void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
01125 {
01126 BT_PROFILE("predictUnconstraintMotion");
01127 for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
01128 {
01129 btRigidBody* body = m_nonStaticRigidBodies[i];
01130 if (!body->isStaticOrKinematicObject())
01131 {
01132
01133
01134
01135 body->applyDamping(timeStep);
01136
01137 body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
01138 }
01139 }
01140 }
01141
01142
01143 void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
01144 {
01145 (void)timeStep;
01146
01147 #ifndef BT_NO_PROFILE
01148 CProfileManager::Reset();
01149 #endif //BT_NO_PROFILE
01150
01151 }
01152
01153
01154
01155
01156
01157
01158 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
01159 {
01160 bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
01161 bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
01162 btScalar dbgDrawSize = constraint->getDbgDrawSize();
01163 if(dbgDrawSize <= btScalar(0.f))
01164 {
01165 return;
01166 }
01167
01168 switch(constraint->getConstraintType())
01169 {
01170 case POINT2POINT_CONSTRAINT_TYPE:
01171 {
01172 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
01173 btTransform tr;
01174 tr.setIdentity();
01175 btVector3 pivot = p2pC->getPivotInA();
01176 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
01177 tr.setOrigin(pivot);
01178 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01179
01180 pivot = p2pC->getPivotInB();
01181 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
01182 tr.setOrigin(pivot);
01183 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01184 }
01185 break;
01186 case HINGE_CONSTRAINT_TYPE:
01187 {
01188 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
01189 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
01190 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01191 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
01192 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01193 btScalar minAng = pHinge->getLowerLimit();
01194 btScalar maxAng = pHinge->getUpperLimit();
01195 if(minAng == maxAng)
01196 {
01197 break;
01198 }
01199 bool drawSect = true;
01200 if(minAng > maxAng)
01201 {
01202 minAng = btScalar(0.f);
01203 maxAng = SIMD_2_PI;
01204 drawSect = false;
01205 }
01206 if(drawLimits)
01207 {
01208 btVector3& center = tr.getOrigin();
01209 btVector3 normal = tr.getBasis().getColumn(2);
01210 btVector3 axis = tr.getBasis().getColumn(0);
01211 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
01212 }
01213 }
01214 break;
01215 case CONETWIST_CONSTRAINT_TYPE:
01216 {
01217 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
01218 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
01219 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01220 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
01221 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01222 if(drawLimits)
01223 {
01224
01225 const btScalar length = dbgDrawSize;
01226 static int nSegments = 8*4;
01227 btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
01228 btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
01229 pPrev = tr * pPrev;
01230 for (int i=0; i<nSegments; i++)
01231 {
01232 fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
01233 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
01234 pCur = tr * pCur;
01235 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
01236
01237 if (i%(nSegments/8) == 0)
01238 getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
01239
01240 pPrev = pCur;
01241 }
01242 btScalar tws = pCT->getTwistSpan();
01243 btScalar twa = pCT->getTwistAngle();
01244 bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
01245 if(useFrameB)
01246 {
01247 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
01248 }
01249 else
01250 {
01251 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
01252 }
01253 btVector3 pivot = tr.getOrigin();
01254 btVector3 normal = tr.getBasis().getColumn(0);
01255 btVector3 axis1 = tr.getBasis().getColumn(1);
01256 getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
01257
01258 }
01259 }
01260 break;
01261 case D6_SPRING_CONSTRAINT_TYPE:
01262 case D6_CONSTRAINT_TYPE:
01263 {
01264 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
01265 btTransform tr = p6DOF->getCalculatedTransformA();
01266 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01267 tr = p6DOF->getCalculatedTransformB();
01268 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01269 if(drawLimits)
01270 {
01271 tr = p6DOF->getCalculatedTransformA();
01272 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
01273 btVector3 up = tr.getBasis().getColumn(2);
01274 btVector3 axis = tr.getBasis().getColumn(0);
01275 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
01276 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
01277 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
01278 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
01279 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
01280 axis = tr.getBasis().getColumn(1);
01281 btScalar ay = p6DOF->getAngle(1);
01282 btScalar az = p6DOF->getAngle(2);
01283 btScalar cy = btCos(ay);
01284 btScalar sy = btSin(ay);
01285 btScalar cz = btCos(az);
01286 btScalar sz = btSin(az);
01287 btVector3 ref;
01288 ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
01289 ref[1] = -sz*axis[0] + cz*axis[1];
01290 ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
01291 tr = p6DOF->getCalculatedTransformB();
01292 btVector3 normal = -tr.getBasis().getColumn(0);
01293 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
01294 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
01295 if(minFi > maxFi)
01296 {
01297 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
01298 }
01299 else if(minFi < maxFi)
01300 {
01301 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
01302 }
01303 tr = p6DOF->getCalculatedTransformA();
01304 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
01305 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
01306 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
01307 }
01308 }
01309 break;
01310 case SLIDER_CONSTRAINT_TYPE:
01311 {
01312 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
01313 btTransform tr = pSlider->getCalculatedTransformA();
01314 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01315 tr = pSlider->getCalculatedTransformB();
01316 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
01317 if(drawLimits)
01318 {
01319 btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
01320 btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
01321 btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
01322 getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
01323 btVector3 normal = tr.getBasis().getColumn(0);
01324 btVector3 axis = tr.getBasis().getColumn(1);
01325 btScalar a_min = pSlider->getLowerAngLimit();
01326 btScalar a_max = pSlider->getUpperAngLimit();
01327 const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
01328 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
01329 }
01330 }
01331 break;
01332 default :
01333 break;
01334 }
01335 return;
01336 }
01337
01338
01339
01340
01341
01342 void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
01343 {
01344 if (m_ownsConstraintSolver)
01345 {
01346 btAlignedFree( m_constraintSolver);
01347 }
01348 m_ownsConstraintSolver = false;
01349 m_constraintSolver = solver;
01350 m_solverIslandCallback->m_solver = solver;
01351 }
01352
01353 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
01354 {
01355 return m_constraintSolver;
01356 }
01357
01358
01359 int btDiscreteDynamicsWorld::getNumConstraints() const
01360 {
01361 return int(m_constraints.size());
01362 }
01363 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
01364 {
01365 return m_constraints[index];
01366 }
01367 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
01368 {
01369 return m_constraints[index];
01370 }
01371
01372
01373
01374 void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
01375 {
01376 int i;
01377
01378 for (i=0;i<m_collisionObjects.size();i++)
01379 {
01380 btCollisionObject* colObj = m_collisionObjects[i];
01381 if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
01382 {
01383 int len = colObj->calculateSerializeBufferSize();
01384 btChunk* chunk = serializer->allocate(len,1);
01385 const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
01386 serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
01387 }
01388 }
01389
01390 for (i=0;i<m_constraints.size();i++)
01391 {
01392 btTypedConstraint* constraint = m_constraints[i];
01393 int size = constraint->calculateSerializeBufferSize();
01394 btChunk* chunk = serializer->allocate(size,1);
01395 const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
01396 serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
01397 }
01398 }
01399
01400
01401
01402
01403 void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
01404 {
01405 #ifdef BT_USE_DOUBLE_PRECISION
01406 int len = sizeof(btDynamicsWorldDoubleData);
01407 btChunk* chunk = serializer->allocate(len,1);
01408 btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
01409 #else
01410 int len = sizeof(btDynamicsWorldFloatData);
01411 btChunk* chunk = serializer->allocate(len,1);
01412 btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
01413 #endif
01414
01415 memset(worldInfo ,0x00,len);
01416
01417 m_gravity.serialize(worldInfo->m_gravity);
01418 worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
01419 worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
01420 worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
01421 worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
01422
01423 worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
01424 worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
01425 worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
01426 worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
01427
01428 worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
01429 worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
01430 worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
01431 worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
01432
01433 worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
01434 worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
01435 worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
01436 worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
01437
01438 worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
01439 worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
01440 worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
01441 worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
01442
01443 worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
01444
01445 #ifdef BT_USE_DOUBLE_PRECISION
01446 const char* structType = "btDynamicsWorldDoubleData";
01447 #else//BT_USE_DOUBLE_PRECISION
01448 const char* structType = "btDynamicsWorldFloatData";
01449 #endif//BT_USE_DOUBLE_PRECISION
01450 serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo);
01451 }
01452
01453 void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
01454 {
01455
01456 serializer->startSerialization();
01457
01458 serializeDynamicsWorldInfo(serializer);
01459
01460 serializeRigidBodies(serializer);
01461
01462 serializeCollisionObjects(serializer);
01463
01464 serializer->finishSerialization();
01465 }
01466