Back to index

supertuxkart  0.5+dfsg1
btDiscreteDynamicsWorld.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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/CollisionShapes/btCollisionShape.h"
00023 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
00024 #include "LinearMath/btTransformUtil.h"
00025 #include "LinearMath/btQuickprof.h"
00026 
00027 //rigidbody & constraints
00028 #include "BulletDynamics/Dynamics/btRigidBody.h"
00029 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
00030 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
00031 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00032 
00033 //for debug rendering
00034 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00035 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
00036 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
00037 #include "BulletCollision/CollisionShapes/btConeShape.h"
00038 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
00039 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
00040 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
00041 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
00042 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00043 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
00044 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
00045 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
00046 #include "LinearMath/btIDebugDraw.h"
00047 
00048 
00049 
00050 //vehicle
00051 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
00052 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
00053 #include "BulletDynamics/Vehicle/btWheelInfo.h"
00054 #include "LinearMath/btIDebugDraw.h"
00055 #include "LinearMath/btQuickprof.h"
00056 #include "LinearMath/btMotionState.h"
00057 
00058 
00059 
00060 
00061 
00062 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
00063 :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
00064 m_constraintSolver(constraintSolver),
00065 m_gravity(0,-10,0),
00066 m_localTime(btScalar(1.)/btScalar(60.)),
00067 m_profileTimings(0)
00068 {
00069        if (!m_constraintSolver)
00070        {
00071               void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
00072               m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
00073               m_ownsConstraintSolver = true;
00074        } else
00075        {
00076               m_ownsConstraintSolver = false;
00077        }
00078 
00079        {
00080               void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
00081               m_islandManager = new (mem) btSimulationIslandManager();
00082        }
00083 
00084        m_ownsIslandManager = true;
00085 }
00086 
00087 
00088 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
00089 {
00090        //only delete it when we created it
00091        if (m_ownsIslandManager)
00092        {
00093               m_islandManager->~btSimulationIslandManager();
00094               btAlignedFree( m_islandManager);
00095        }
00096        if (m_ownsConstraintSolver)
00097        {
00098 
00099               m_constraintSolver->~btConstraintSolver();
00100               btAlignedFree(m_constraintSolver);
00101        }
00102 }
00103 
00104 void   btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
00105 {
00106 
00107        for (int i=0;i<m_collisionObjects.size();i++)
00108        {
00109               btCollisionObject* colObj = m_collisionObjects[i];
00110               btRigidBody* body = btRigidBody::upcast(colObj);
00111               if (body)
00112               {
00113                             btTransform predictedTrans;
00114                             if (body->getActivationState() != ISLAND_SLEEPING)
00115                             {
00116                                    if (body->isKinematicObject())
00117                                    {
00118                                           //to calculate velocities next frame
00119                                           body->saveKinematicState(timeStep);
00120                                    }
00121                             }
00122               }
00123        }
00124 }
00125 
00126 void   btDiscreteDynamicsWorld::debugDrawWorld()
00127 {
00128        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
00129        {
00130               int i;
00131 
00132               //todo: iterate over awake simulation islands!
00133               for (  i=0;i<m_collisionObjects.size();i++)
00134               {
00135                      btCollisionObject* colObj = m_collisionObjects[i];
00136                      if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
00137                      {
00138                             btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
00139                             switch(colObj->getActivationState())
00140                             {
00141                             case  ACTIVE_TAG:
00142                                    color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
00143                             case ISLAND_SLEEPING:
00144                                    color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
00145                             case WANTS_DEACTIVATION:
00146                                    color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
00147                             case DISABLE_DEACTIVATION:
00148                                    color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
00149                             case DISABLE_SIMULATION:
00150                                    color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
00151                             default:
00152                                    {
00153                                           color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
00154                                    }
00155                             };
00156 
00157                             debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
00158                      }
00159                      if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
00160                      {
00161                             btPoint3 minAabb,maxAabb;
00162                             btVector3 colorvec(1,0,0);
00163                             colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
00164                             m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
00165                      }
00166 
00167               }
00168        
00169               for (  i=0;i<this->m_vehicles.size();i++)
00170               {
00171                      for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
00172                      {
00173                             btVector3 wheelColor(0,255,255);
00174                             if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
00175                             {
00176                                    wheelColor.setValue(0,0,255);
00177                             } else
00178                             {
00179                                    wheelColor.setValue(255,0,255);
00180                             }
00181               
00182                             btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
00183 
00184                             btVector3 axle = btVector3( 
00185                                    m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
00186                                    m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
00187                                    m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
00188 
00189 
00190                             //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
00191                             //debug wheels (cylinders)
00192                             m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
00193                             m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
00194 
00195                      }
00196               }
00197        }
00198 }
00199 
00200 void   btDiscreteDynamicsWorld::clearForces()
00201 {
00202        //todo: iterate over awake simulation islands!
00203        for ( int i=0;i<m_collisionObjects.size();i++)
00204        {
00205               btCollisionObject* colObj = m_collisionObjects[i];
00206               
00207               btRigidBody* body = btRigidBody::upcast(colObj);
00208               if (body)
00209               {
00210                      body->clearForces();
00211               }
00212        }
00213 }      
00214 
00216 void   btDiscreteDynamicsWorld::applyGravity()
00217 {
00218        //todo: iterate over awake simulation islands!
00219        for ( int i=0;i<m_collisionObjects.size();i++)
00220        {
00221               btCollisionObject* colObj = m_collisionObjects[i];
00222               
00223               btRigidBody* body = btRigidBody::upcast(colObj);
00224               if (body && body->isActive())
00225               {
00226                      body->applyGravity();
00227               }
00228        }
00229 }
00230 
00231 
00232 
00233 void   btDiscreteDynamicsWorld::synchronizeMotionStates()
00234 {
00235        {
00236               //todo: iterate over awake simulation islands!
00237               for ( int i=0;i<m_collisionObjects.size();i++)
00238               {
00239                      btCollisionObject* colObj = m_collisionObjects[i];
00240                      
00241                      btRigidBody* body = btRigidBody::upcast(colObj);
00242                      if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
00243                      {
00244                             //we need to call the update at least once, even for sleeping objects
00245                             //otherwise the 'graphics' transform never updates properly
00246                             //so todo: add 'dirty' flag
00247                             //if (body->getActivationState() != ISLAND_SLEEPING)
00248                             {
00249                                    btTransform interpolatedTransform;
00250                                    btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
00251                                           body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
00252                                    body->getMotionState()->setWorldTransform(interpolatedTransform);
00253                             }
00254                      }
00255               }
00256        }
00257 
00258        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
00259        {
00260               for ( int i=0;i<this->m_vehicles.size();i++)
00261               {
00262                      for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
00263                      {
00264                             //synchronize the wheels with the (interpolated) chassis worldtransform
00265                             m_vehicles[i]->updateWheelTransform(v,true);
00266                      }
00267               }
00268        }
00269 
00270 }
00271 
00272 
00273 int    btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
00274 {
00275        startProfiling(timeStep);
00276 
00277        BT_PROFILE("stepSimulation");
00278 
00279        int numSimulationSubSteps = 0;
00280 
00281        if (maxSubSteps)
00282        {
00283               //fixed timestep with interpolation
00284               m_localTime += timeStep;
00285               if (m_localTime >= fixedTimeStep)
00286               {
00287                      numSimulationSubSteps = int( m_localTime / fixedTimeStep);
00288                      m_localTime -= numSimulationSubSteps * fixedTimeStep;
00289               }
00290        } else
00291        {
00292               //variable timestep
00293               fixedTimeStep = timeStep;
00294               m_localTime = timeStep;
00295               if (btFuzzyZero(timeStep))
00296               {
00297                      numSimulationSubSteps = 0;
00298                      maxSubSteps = 0;
00299               } else
00300               {
00301                      numSimulationSubSteps = 1;
00302                      maxSubSteps = 1;
00303               }
00304        }
00305 
00306        //process some debugging flags
00307        if (getDebugDrawer())
00308        {
00309               gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
00310        }
00311        if (numSimulationSubSteps)
00312        {
00313 
00314               saveKinematicState(fixedTimeStep);
00315 
00316               applyGravity();
00317 
00318               //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
00319               int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
00320 
00321               for (int i=0;i<clampedSimulationSteps;i++)
00322               {
00323                      internalSingleStepSimulation(fixedTimeStep);
00324                      synchronizeMotionStates();
00325               }
00326 
00327        } 
00328 
00329        synchronizeMotionStates();
00330 
00331        clearForces();
00332 
00333 #ifndef BT_NO_PROFILE
00334        CProfileManager::Increment_Frame_Counter();
00335 #endif //BT_NO_PROFILE
00336        
00337        return numSimulationSubSteps;
00338 }
00339 
00340 void   btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
00341 {
00342        
00343        BT_PROFILE("internalSingleStepSimulation");
00344 
00346        predictUnconstraintMotion(timeStep);
00347 
00348        btDispatcherInfo& dispatchInfo = getDispatchInfo();
00349 
00350        dispatchInfo.m_timeStep = timeStep;
00351        dispatchInfo.m_stepCount = 0;
00352        dispatchInfo.m_debugDraw = getDebugDrawer();
00353 
00355        performDiscreteCollisionDetection();
00356 
00357        calculateSimulationIslands();
00358 
00359        
00360        getSolverInfo().m_timeStep = timeStep;
00361        
00362 
00363 
00365        solveConstraints(getSolverInfo());
00366        
00368 
00370        integrateTransforms(timeStep);
00371 
00373        updateVehicles(timeStep);
00374 
00375 
00376        updateActivationState( timeStep );
00377 
00378        if(0 != m_internalTickCallback) {
00379               (*m_internalTickCallback)(this, timeStep);
00380        }      
00381 }
00382 
00383 void   btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
00384 {
00385        m_gravity = gravity;
00386        for ( int i=0;i<m_collisionObjects.size();i++)
00387        {
00388               btCollisionObject* colObj = m_collisionObjects[i];
00389               btRigidBody* body = btRigidBody::upcast(colObj);
00390               if (body)
00391               {
00392                      body->setGravity(gravity);
00393               }
00394        }
00395 }
00396 
00397 btVector3 btDiscreteDynamicsWorld::getGravity () const
00398 {
00399        return m_gravity;
00400 }
00401 
00402 
00403 void   btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
00404 {
00405        removeCollisionObject(body);
00406 }
00407 
00408 void   btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
00409 {
00410        if (!body->isStaticOrKinematicObject())
00411        {
00412               body->setGravity(m_gravity);
00413        }
00414 
00415        if (body->getCollisionShape())
00416        {
00417               bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
00418               short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
00419               short collisionFilterMask = isDynamic?    short(btBroadphaseProxy::AllFilter) :     short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
00420 
00421               addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
00422        }
00423 }
00424 
00425 void   btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
00426 {
00427        if (!body->isStaticOrKinematicObject())
00428        {
00429               body->setGravity(m_gravity);
00430        }
00431 
00432        if (body->getCollisionShape())
00433        {
00434               addCollisionObject(body,group,mask);
00435        }
00436 }
00437 
00438 
00439 void   btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
00440 {
00441        BT_PROFILE("updateVehicles");
00442        
00443        for ( int i=0;i<m_vehicles.size();i++)
00444        {
00445               btRaycastVehicle* vehicle = m_vehicles[i];
00446               vehicle->updateVehicle( timeStep);
00447        }
00448 }
00449 
00450 void   btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
00451 {
00452        BT_PROFILE("updateActivationState");
00453 
00454        for ( int i=0;i<m_collisionObjects.size();i++)
00455        {
00456               btCollisionObject* colObj = m_collisionObjects[i];
00457               btRigidBody* body = btRigidBody::upcast(colObj);
00458               if (body)
00459               {
00460                      body->updateDeactivation(timeStep);
00461 
00462                      if (body->wantsSleeping())
00463                      {
00464                             if (body->isStaticOrKinematicObject())
00465                             {
00466                                    body->setActivationState(ISLAND_SLEEPING);
00467                             } else
00468                             {
00469                                    if (body->getActivationState() == ACTIVE_TAG)
00470                                           body->setActivationState( WANTS_DEACTIVATION );
00471                             }
00472                      } else
00473                      {
00474                             if (body->getActivationState() != DISABLE_DEACTIVATION)
00475                                    body->setActivationState( ACTIVE_TAG );
00476                      }
00477               }
00478        }
00479 }
00480 
00481 void   btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
00482 {
00483        m_constraints.push_back(constraint);
00484        if (disableCollisionsBetweenLinkedBodies)
00485        {
00486               constraint->getRigidBodyA().addConstraintRef(constraint);
00487               constraint->getRigidBodyB().addConstraintRef(constraint);
00488        }
00489 }
00490 
00491 void   btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
00492 {
00493        m_constraints.remove(constraint);
00494        constraint->getRigidBodyA().removeConstraintRef(constraint);
00495        constraint->getRigidBodyB().removeConstraintRef(constraint);
00496 }
00497 
00498 void   btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
00499 {
00500        m_vehicles.push_back(vehicle);
00501 }
00502 
00503 void   btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
00504 {
00505        m_vehicles.remove(vehicle);
00506 }
00507 
00508 SIMD_FORCE_INLINE    int    btGetConstraintIslandId(const btTypedConstraint* lhs)
00509 {
00510        int islandId;
00511        
00512        const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
00513        const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
00514        islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
00515        return islandId;
00516 
00517 }
00518 
00519 
00520 class btSortConstraintOnIslandPredicate
00521 {
00522        public:
00523 
00524               bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
00525               {
00526                      int rIslandId0,lIslandId0;
00527                      rIslandId0 = btGetConstraintIslandId(rhs);
00528                      lIslandId0 = btGetConstraintIslandId(lhs);
00529                      return lIslandId0 < rIslandId0;
00530               }
00531 };
00532 
00533 
00534 
00535 
00536 void   btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
00537 {
00538        BT_PROFILE("solveConstraints");
00539        
00540        struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
00541        {
00542 
00543               btContactSolverInfo& m_solverInfo;
00544               btConstraintSolver*         m_solver;
00545               btTypedConstraint**         m_sortedConstraints;
00546               int                                       m_numConstraints;
00547               btIDebugDraw*               m_debugDrawer;
00548               btStackAlloc*               m_stackAlloc;
00549               btDispatcher*               m_dispatcher;
00550 
00551               InplaceSolverIslandCallback(
00552                      btContactSolverInfo& solverInfo,
00553                      btConstraintSolver*  solver,
00554                      btTypedConstraint** sortedConstraints,
00555                      int    numConstraints,
00556                      btIDebugDraw* debugDrawer,
00557                      btStackAlloc*               stackAlloc,
00558                      btDispatcher* dispatcher)
00559                      :m_solverInfo(solverInfo),
00560                      m_solver(solver),
00561                      m_sortedConstraints(sortedConstraints),
00562                      m_numConstraints(numConstraints),
00563                      m_debugDrawer(debugDrawer),
00564                      m_stackAlloc(stackAlloc),
00565                      m_dispatcher(dispatcher)
00566               {
00567 
00568               }
00569 
00570               InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
00571               {
00572                      btAssert(0);
00573                      (void)other;
00574                      return *this;
00575               }
00576               virtual       void   ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**       manifolds,int numManifolds, int islandId)
00577               {
00578                      if (islandId<0)
00579                      {
00581                             m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
00582                      } else
00583                      {
00584                                    //also add all non-contact constraints/joints for this island
00585                             btTypedConstraint** startConstraint = 0;
00586                             int numCurConstraints = 0;
00587                             int i;
00588                             
00589                             //find the first constraint for this island
00590                             for (i=0;i<m_numConstraints;i++)
00591                             {
00592                                    if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
00593                                    {
00594                                           startConstraint = &m_sortedConstraints[i];
00595                                           break;
00596                                    }
00597                             }
00598                             //count the number of constraints in this island
00599                             for (;i<m_numConstraints;i++)
00600                             {
00601                                    if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
00602                                    {
00603                                           numCurConstraints++;
00604                                    }
00605                             }
00606 
00608                             if (numManifolds + numCurConstraints)
00609                             {
00610                                    m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
00611                             }
00612               
00613                      }
00614               }
00615 
00616        };
00617 
00618        //sorted version of all btTypedConstraint, based on islandId
00619        btAlignedObjectArray<btTypedConstraint*>  sortedConstraints;
00620        sortedConstraints.resize( m_constraints.size());
00621        int i; 
00622        for (i=0;i<getNumConstraints();i++)
00623        {
00624               sortedConstraints[i] = m_constraints[i];
00625        }
00626 
00627 //     assert(0);
00628               
00629        
00630 
00631        sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
00632        
00633        btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
00634        
00635        InplaceSolverIslandCallback solverCallback(      solverInfo,   m_constraintSolver, constraintsPtr,sortedConstraints.size(),   m_debugDrawer,m_stackAlloc,m_dispatcher1);
00636        
00637        m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
00638        
00640        m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
00641 
00642        m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
00643 }
00644 
00645 
00646 
00647 
00648 void   btDiscreteDynamicsWorld::calculateSimulationIslands()
00649 {
00650        BT_PROFILE("calculateSimulationIslands");
00651 
00652        getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
00653 
00654        {
00655               int i;
00656               int numConstraints = int(m_constraints.size());
00657               for (i=0;i< numConstraints ; i++ )
00658               {
00659                      btTypedConstraint* constraint = m_constraints[i];
00660 
00661                      const btRigidBody* colObj0 = &constraint->getRigidBodyA();
00662                      const btRigidBody* colObj1 = &constraint->getRigidBodyB();
00663 
00664                      if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
00665                             ((colObj1) && ((colObj1)->mergesSimulationIslands())))
00666                      {
00667                             if (colObj0->isActive() || colObj1->isActive())
00668                             {
00669 
00670                                    getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
00671                                           (colObj1)->getIslandTag());
00672                             }
00673                      }
00674               }
00675        }
00676 
00677        //Store the island id in each body
00678        getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
00679 
00680        
00681 }
00682 
00683 
00684 
00685 void   btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
00686 {
00687        BT_PROFILE("integrateTransforms");
00688        btTransform predictedTrans;
00689        for ( int i=0;i<m_collisionObjects.size();i++)
00690        {
00691               btCollisionObject* colObj = m_collisionObjects[i];
00692               btRigidBody* body = btRigidBody::upcast(colObj);
00693               if (body)
00694               {
00695                      if (body->isActive() && (!body->isStaticOrKinematicObject()))
00696                      {
00697                             body->predictIntegratedTransform(timeStep, predictedTrans);
00698                             body->proceedToTransform( predictedTrans);
00699                      }
00700               }
00701        }
00702 }
00703 
00704 
00705 
00706 void   btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
00707 {
00708        BT_PROFILE("predictUnconstraintMotion");
00709        for ( int i=0;i<m_collisionObjects.size();i++)
00710        {
00711               btCollisionObject* colObj = m_collisionObjects[i];
00712               btRigidBody* body = btRigidBody::upcast(colObj);
00713               if (body)
00714               {
00715                      if (!body->isStaticOrKinematicObject())
00716                      {
00717                             if (body->isActive())
00718                             {
00719                                    body->integrateVelocities( timeStep);
00720                                    //damping
00721                                    body->applyDamping(timeStep);
00722 
00723                                    body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
00724                             }
00725                      }
00726               }
00727        }
00728 }
00729 
00730 
00731 void   btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
00732 {
00733        (void)timeStep;
00734 
00735 #ifndef BT_NO_PROFILE
00736        CProfileManager::Reset();
00737 #endif //BT_NO_PROFILE
00738 
00739 }
00740 
00741 
00742 
00743 
00744        
00745 
00746 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
00747 {
00748        btIDebugDraw* m_debugDrawer;
00749        btVector3     m_color;
00750        btTransform   m_worldTrans;
00751 
00752 public:
00753 
00754        DebugDrawcallback(btIDebugDraw*    debugDrawer,const btTransform& worldTrans,const btVector3& color) :
00755                 m_debugDrawer(debugDrawer),
00756               m_color(color),
00757               m_worldTrans(worldTrans)
00758        {
00759        }
00760 
00761        virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
00762        {
00763               processTriangle(triangle,partId,triangleIndex);
00764        }
00765 
00766        virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
00767        {
00768               (void)partId;
00769               (void)triangleIndex;
00770 
00771               btVector3 wv0,wv1,wv2;
00772               wv0 = m_worldTrans*triangle[0];
00773               wv1 = m_worldTrans*triangle[1];
00774               wv2 = m_worldTrans*triangle[2];
00775               m_debugDrawer->drawLine(wv0,wv1,m_color);
00776               m_debugDrawer->drawLine(wv1,wv2,m_color);
00777               m_debugDrawer->drawLine(wv2,wv0,m_color);
00778        }
00779 };
00780 
00781 void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
00782 {
00783        btVector3 start = transform.getOrigin();
00784 
00785        const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
00786        const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
00787        const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
00788 
00789        // XY 
00790        getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
00791        getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
00792        getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
00793        getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
00794 
00795        // XZ
00796        getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
00797        getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
00798        getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
00799        getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
00800 
00801        // YZ
00802        getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
00803        getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
00804        getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
00805        getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
00806 }
00807 
00808 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
00809 {
00810        // Draw a small simplex at the center of the object
00811        {
00812               btVector3 start = worldTransform.getOrigin();
00813               getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
00814               getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
00815               getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
00816        }
00817 
00818        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
00819        {
00820               const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
00821               for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
00822               {
00823                      btTransform childTrans = compoundShape->getChildTransform(i);
00824                      const btCollisionShape* colShape = compoundShape->getChildShape(i);
00825                      debugDrawObject(worldTransform*childTrans,colShape,color);
00826               }
00827 
00828        } else
00829        {
00830               switch (shape->getShapeType())
00831               {
00832 
00833               case SPHERE_SHAPE_PROXYTYPE:
00834                      {
00835                             const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
00836                             btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
00837                             
00838                             debugDrawSphere(radius, worldTransform, color);
00839                             break;
00840                      }
00841               case MULTI_SPHERE_SHAPE_PROXYTYPE:
00842                      {
00843                             const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
00844 
00845                             for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
00846                             {
00847                                    btTransform childTransform = worldTransform;
00848                                    childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
00849                                    debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
00850                             }
00851 
00852                             break;
00853                      }
00854               case CAPSULE_SHAPE_PROXYTYPE:
00855                      {
00856                             const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
00857 
00858                             btScalar radius = capsuleShape->getRadius();
00859                             btScalar halfHeight = capsuleShape->getHalfHeight();
00860 
00861                             // Draw the ends
00862                             {
00863                                    btTransform childTransform = worldTransform;
00864                                    childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
00865                                    debugDrawSphere(radius, childTransform, color);
00866                             }
00867 
00868                             {
00869                                    btTransform childTransform = worldTransform;
00870                                    childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
00871                                    debugDrawSphere(radius, childTransform, color);
00872                             }
00873 
00874                             // Draw some additional lines
00875                             btVector3 start = worldTransform.getOrigin();
00876                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
00877                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
00878                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
00879                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
00880 
00881                             break;
00882                      }
00883               case CONE_SHAPE_PROXYTYPE:
00884                      {
00885                             const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
00886                             btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
00887                             btScalar height = coneShape->getHeight();//+coneShape->getMargin();
00888                             btVector3 start = worldTransform.getOrigin();
00889 
00890                             int upAxis= coneShape->getConeUpIndex();
00891                             
00892 
00893                             btVector3     offsetHeight(0,0,0);
00894                             offsetHeight[upAxis] = height * btScalar(0.5);
00895                             btVector3     offsetRadius(0,0,0);
00896                             offsetRadius[(upAxis+1)%3] = radius;
00897                             btVector3     offset2Radius(0,0,0);
00898                             offset2Radius[(upAxis+2)%3] = radius;
00899 
00900                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
00901                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
00902                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
00903                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
00904 
00905 
00906 
00907                             break;
00908 
00909                      }
00910               case CYLINDER_SHAPE_PROXYTYPE:
00911                      {
00912                             const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
00913                             int upAxis = cylinder->getUpAxis();
00914                             btScalar radius = cylinder->getRadius();
00915                             btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
00916                             btVector3 start = worldTransform.getOrigin();
00917                             btVector3     offsetHeight(0,0,0);
00918                             offsetHeight[upAxis] = halfHeight;
00919                             btVector3     offsetRadius(0,0,0);
00920                             offsetRadius[(upAxis+1)%3] = radius;
00921                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
00922                             getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
00923                             break;
00924                      }
00925 
00926                      case STATIC_PLANE_PROXYTYPE:
00927                             {
00928                                    const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
00929                                    btScalar planeConst = staticPlaneShape->getPlaneConstant();
00930                                    const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
00931                                    btVector3 planeOrigin = planeNormal * planeConst;
00932                                    btVector3 vec0,vec1;
00933                                    btPlaneSpace1(planeNormal,vec0,vec1);
00934                                    btScalar vecLen = 100.f;
00935                                    btVector3 pt0 = planeOrigin + vec0*vecLen;
00936                                    btVector3 pt1 = planeOrigin - vec0*vecLen;
00937                                    btVector3 pt2 = planeOrigin + vec1*vecLen;
00938                                    btVector3 pt3 = planeOrigin - vec1*vecLen;
00939                                    getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
00940                                    getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
00941                                    break;
00942 
00943                             }
00944               default:
00945                      {
00946 
00947                             if (shape->isConcave())
00948                             {
00949                                    btConcaveShape* concaveMesh = (btConcaveShape*) shape;
00950                                    
00951                                    //todo pass camera, for some culling
00952                                    btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
00953                                    btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
00954 
00955                                    DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
00956                                    concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
00957 
00958                             }
00959 
00960                             if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
00961                             {
00962                                    btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
00963                                    //todo: pass camera for some culling                    
00964                                    btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
00965                                    btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
00966                                    //DebugDrawcallback drawCallback;
00967                                    DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
00968                                    convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
00969                             }
00970 
00971 
00973                             if (shape->isPolyhedral())
00974                             {
00975                                    btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
00976 
00977                                    int i;
00978                                    for (i=0;i<polyshape->getNumEdges();i++)
00979                                    {
00980                                           btPoint3 a,b;
00981                                           polyshape->getEdge(i,a,b);
00982                                           btVector3 wa = worldTransform * a;
00983                                           btVector3 wb = worldTransform * b;
00984                                           getDebugDrawer()->drawLine(wa,wb,color);
00985 
00986                                    }
00987 
00988                                    
00989                             }
00990                      }
00991               }
00992        }
00993 }
00994 
00995 
00996 void   btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
00997 {
00998        if (m_ownsConstraintSolver)
00999        {
01000               btAlignedFree( m_constraintSolver);
01001        }
01002        m_ownsConstraintSolver = false;
01003        m_constraintSolver = solver;
01004 }
01005 
01006 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
01007 {
01008        return m_constraintSolver;
01009 }
01010 
01011 
01012 int           btDiscreteDynamicsWorld::getNumConstraints() const
01013 {
01014        return int(m_constraints.size());
01015 }
01016 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
01017 {
01018        return m_constraints[index];
01019 }
01020 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
01021 {
01022        return m_constraints[index];
01023 }