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_dispatcher1); 82 m_constraintSolver->allSolved(infoGlobal,m_debugDrawer); 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 } 281
