btDiscreteDynamicsWorld.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btDiscreteDynamicsWorld.h"
00018 
00019 //collision detection
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 //rigidbody & constraints
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                                 //also add all non-contact constraints/joints for this island
00142                         btTypedConstraint** startConstraint = 0;
00143                         int numCurConstraints = 0;
00144                         int i;
00145                         
00146                         //find the first constraint for this island
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                         //count the number of constraints in this island
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                                         //printf("deferred\n");
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         //only delete it when we created it
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                                 //to calculate velocities next frame
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                 //need to check if next line is ok
00329                 //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
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                 //we need to call the update at least once, even for sleeping objects
00356                 //otherwise the 'graphics' transform never updates properly
00358                 //if (body->getActivationState() != ISLAND_SLEEPING)
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                 //iterate  over all collision objects
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                 //iterate over all active rigid bodies
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                 //fixed timestep with interpolation
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                 //variable timestep
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         //process some debugging flags
00429         if (getDebugDrawer())
00430         {
00431                 btIDebugDraw* debugDrawer = getDebugDrawer ();
00432                 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
00433         }
00434         if (numSimulationSubSteps)
00435         {
00436 
00437                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
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 //      btAssert(0);
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         //merge islands based on speculative contact manifolds too
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         //Store the island id in each body
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                 //ignore result if there is no contact response
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);//toB.getOrigin()-fromB.getOrigin();
00824 
00825                 btVector3 relativeVelocity = (linVelA-linVelB);
00826                 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
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                 //don't collide with itself
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                 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
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                                         //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
00938                                         btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
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                                         //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
01024                                         btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
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                                                 //printf("clamped integration to hit fraction = %f\n",fraction);
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                                                 //don't apply the collision response right now, it will happen next frame
01062                                                 //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
01063                                                 //btScalar appliedImpulse = 0.f;
01064                                                 //btScalar depth = 0.f;
01065                                                 //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
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                                 //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
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                         //don't integrate/update velocities here, it happens in the constraint solver
01133 
01134                         //damping
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                                 // that ideally should draw the same frame      
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                                         //const btScalar length = btScalar(5);
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         //serialize all collision objects
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//BT_USE_DOUBLE_PRECISION
01410                 int len = sizeof(btDynamicsWorldFloatData);
01411                 btChunk* chunk = serializer->allocate(len,1);
01412                 btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
01413 #endif//BT_USE_DOUBLE_PRECISION
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