src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btSimpleDynamicsWorld.h"
  17 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  18 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
  19 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
  20 #include "BulletDynamics/Dynamics/btRigidBody.h"
  21 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
  22 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
  23
  24
  25 /*
  26   Make sure this dummy function never changes so that it
  27   can be used by probes that are checking whether the
  28   library is actually installed.
  29 */
  30 extern "C"
  31 {
  32         void btBulletDynamicsProbe ();
  33         void btBulletDynamicsProbe () {}
  34 }
  35
  36
  37
  38
  39 btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
  40 :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
  41 m_constraintSolver(constraintSolver),
  42 m_ownsConstraintSolver(false),
  43 m_gravity(0,0,-10)
  44 {
  45
  46 }
  47
  48
  49 btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
  50 {
  51         if (m_ownsConstraintSolver)
  52                 btAlignedFree( m_constraintSolver);
  53 }
  54
  55 int             btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
  56 {
  57         (void)fixedTimeStep;
  58         (void)maxSubSteps;
  59
  60
  61         ///apply gravity, predict motion
  62         predictUnconstraintMotion(timeStep);
  63
  64         btDispatcherInfo&       dispatchInfo = getDispatchInfo();
  65         dispatchInfo.m_timeStep = timeStep;
  66         dispatchInfo.m_stepCount = 0;
  67         dispatchInfo.m_debugDraw = getDebugDrawer();
  68
  69         ///perform collision detection
  70         performDiscreteCollisionDetection();
  71
  72         ///solve contact constraints
  73         int numManifolds = m_dispatcher1->getNumManifolds();
  74         if (numManifolds)
  75         {
  76                 btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
  77
  78                 btContactSolverInfo infoGlobal;
  79                 infoGlobal.m_timeStep = timeStep;
  80                 m_constraintSolver->prepareSolve(0,numManifolds);
  81                 m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
  82                 m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
  83         }
  84
  85         ///integrate transforms
  86         integrateTransforms(timeStep);
  87
  88         updateAabbs();
  89
  90         synchronizeMotionStates();
  91
  92         clearForces();
  93
  94         return 1;
  95
  96 }
  97
  98 void    btSimpleDynamicsWorld::clearForces()
  99 {
 100         ///@todo: iterate over awake simulation islands!
 101         for ( int i=0;i<m_collisionObjects.size();i++)
 102         {
 103                 btCollisionObject* colObj = m_collisionObjects[i];
 104
 105                 btRigidBody* body = btRigidBody::upcast(colObj);
 106                 if (body)
 107                 {
 108                         body->clearForces();
 109                 }
 110         }
 111 }
 112
 113
 114 void    btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 115 {
 116         m_gravity = gravity;
 117         for ( int i=0;i<m_collisionObjects.size();i++)
 118         {
 119                 btCollisionObject* colObj = m_collisionObjects[i];
 120                 btRigidBody* body = btRigidBody::upcast(colObj);
 121                 if (body)
 122                 {
 123                         body->setGravity(gravity);
 124                 }
 125         }
 126 }
 127
 128 btVector3 btSimpleDynamicsWorld::getGravity () const
 129 {
 130         return m_gravity;
 131 }
 132
 133 void    btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 134 {
 135         btCollisionWorld::removeCollisionObject(body);
 136 }
 137
 138 void    btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 139 {
 140         btRigidBody* body = btRigidBody::upcast(collisionObject);
 141         if (body)
 142                 removeRigidBody(body);
 143         else
 144                 btCollisionWorld::removeCollisionObject(collisionObject);
 145 }
 146
 147
 148 void    btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 149 {
 150         body->setGravity(m_gravity);
 151
 152         if (body->getCollisionShape())
 153         {
 154                 addCollisionObject(body);
 155         }
 156 }
 157
 158 void    btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
 159 {
 160         body->setGravity(m_gravity);
 161
 162         if (body->getCollisionShape())
 163         {
 164                 addCollisionObject(body,group,mask);
 165         }
 166 }
 167
 168
 169 void    btSimpleDynamicsWorld::debugDrawWorld()
 170 {
 171
 172 }
 173
 174 void    btSimpleDynamicsWorld::addAction(btActionInterface* action)
 175 {
 176
 177 }
 178
 179 void    btSimpleDynamicsWorld::removeAction(btActionInterface* action)
 180 {
 181
 182 }
 183
 184
 185 void    btSimpleDynamicsWorld::updateAabbs()
 186 {
 187         btTransform predictedTrans;
 188         for ( int i=0;i<m_collisionObjects.size();i++)
 189         {
 190                 btCollisionObject* colObj = m_collisionObjects[i];
 191                 btRigidBody* body = btRigidBody::upcast(colObj);
 192                 if (body)
 193                 {
 194                         if (body->isActive() && (!body->isStaticObject()))
 195                         {
 196                                 btVector3 minAabb,maxAabb;
 197                                 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 198                                 btBroadphaseInterface* bp = getBroadphase();
 199                                 bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 200                         }
 201                 }
 202         }
 203 }
 204
 205 void    btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 206 {
 207         btTransform predictedTrans;
 208         for ( int i=0;i<m_collisionObjects.size();i++)
 209         {
 210                 btCollisionObject* colObj = m_collisionObjects[i];
 211                 btRigidBody* body = btRigidBody::upcast(colObj);
 212                 if (body)
 213                 {
 214                         if (body->isActive() && (!body->isStaticObject()))
 215                         {
 216                                 body->predictIntegratedTransform(timeStep, predictedTrans);
 217                                 body->proceedToTransform( predictedTrans);
 218                         }
 219                 }
 220         }
 221 }
 222
 223
 224
 225 void    btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 226 {
 227         for ( int i=0;i<m_collisionObjects.size();i++)
 228         {
 229                 btCollisionObject* colObj = m_collisionObjects[i];
 230                 btRigidBody* body = btRigidBody::upcast(colObj);
 231                 if (body)
 232                 {
 233                         if (!body->isStaticObject())
 234                         {
 235                                 if (body->isActive())
 236                                 {
 237                                         body->applyGravity();
 238                                         body->integrateVelocities( timeStep);
 239                                         body->applyDamping(timeStep);
 240                                         body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 241                                 }
 242                         }
 243                 }
 244         }
 245 }
 246
 247
 248 void    btSimpleDynamicsWorld::synchronizeMotionStates()
 249 {
 250         ///@todo: iterate over awake simulation islands!
 251         for ( int i=0;i<m_collisionObjects.size();i++)
 252         {
 253                 btCollisionObject* colObj = m_collisionObjects[i];
 254                 btRigidBody* body = btRigidBody::upcast(colObj);
 255                 if (body && body->getMotionState())
 256                 {
 257                         if (body->getActivationState() != ISLAND_SLEEPING)
 258                         {
 259                                 body->getMotionState()->setWorldTransform(body->getWorldTransform());
 260                         }
 261                 }
 262         }
 263
 264 }
 265
 266
 267 void    btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 268 {
 269         if (m_ownsConstraintSolver)
 270         {
 271                 btAlignedFree(m_constraintSolver);
 272         }
 273         m_ownsConstraintSolver = false;
 274         m_constraintSolver = solver;
 275 }
 276
 277 btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
 278 {
 279         return m_constraintSolver;
 280 }