dali-physics/third-party/bullet3/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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   Make sure this dummy function never changes so that it
  26   can be used by probes that are checking whether the
  27   library is actually installed.
  28 */
  29 extern "C"
  30 {
  31         void btBulletDynamicsProbe();
  32         void btBulletDynamicsProbe() {}
  33 }
  34
  35 btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
  36         : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
  37           m_constraintSolver(constraintSolver),
  38           m_ownsConstraintSolver(false),
  39           m_gravity(0, 0, -10)
  40 {
  41 }
  42
  43 btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
  44 {
  45         if (m_ownsConstraintSolver)
  46                 btAlignedFree(m_constraintSolver);
  47 }
  48
  49 int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
  50 {
  51         (void)fixedTimeStep;
  52         (void)maxSubSteps;
  53
  54         ///apply gravity, predict motion
  55         predictUnconstraintMotion(timeStep);
  56
  57         btDispatcherInfo& dispatchInfo = getDispatchInfo();
  58         dispatchInfo.m_timeStep = timeStep;
  59         dispatchInfo.m_stepCount = 0;
  60         dispatchInfo.m_debugDraw = getDebugDrawer();
  61
  62         ///perform collision detection
  63         performDiscreteCollisionDetection();
  64
  65         ///solve contact constraints
  66         int numManifolds = m_dispatcher1->getNumManifolds();
  67         if (numManifolds)
  68         {
  69                 btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
  70
  71                 btContactSolverInfo infoGlobal;
  72                 infoGlobal.m_timeStep = timeStep;
  73                 m_constraintSolver->prepareSolve(0, numManifolds);
  74                 m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
  75                 m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
  76         }
  77
  78         ///integrate transforms
  79         integrateTransforms(timeStep);
  80
  81         updateAabbs();
  82
  83         synchronizeMotionStates();
  84
  85         clearForces();
  86
  87         return 1;
  88 }
  89
  90 void btSimpleDynamicsWorld::clearForces()
  91 {
  92         ///@todo: iterate over awake simulation islands!
  93         for (int i = 0; i < m_collisionObjects.size(); i++)
  94         {
  95                 btCollisionObject* colObj = m_collisionObjects[i];
  96
  97                 btRigidBody* body = btRigidBody::upcast(colObj);
  98                 if (body)
  99                 {
 100                         body->clearForces();
 101                 }
 102         }
 103 }
 104
 105 void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 106 {
 107         m_gravity = gravity;
 108         for (int i = 0; i < m_collisionObjects.size(); i++)
 109         {
 110                 btCollisionObject* colObj = m_collisionObjects[i];
 111                 btRigidBody* body = btRigidBody::upcast(colObj);
 112                 if (body)
 113                 {
 114                         body->setGravity(gravity);
 115                 }
 116         }
 117 }
 118
 119 btVector3 btSimpleDynamicsWorld::getGravity() const
 120 {
 121         return m_gravity;
 122 }
 123
 124 void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 125 {
 126         btCollisionWorld::removeCollisionObject(body);
 127 }
 128
 129 void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 130 {
 131         btRigidBody* body = btRigidBody::upcast(collisionObject);
 132         if (body)
 133                 removeRigidBody(body);
 134         else
 135                 btCollisionWorld::removeCollisionObject(collisionObject);
 136 }
 137
 138 void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 139 {
 140         body->setGravity(m_gravity);
 141
 142         if (body->getCollisionShape())
 143         {
 144                 addCollisionObject(body);
 145         }
 146 }
 147
 148 void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 149 {
 150         body->setGravity(m_gravity);
 151
 152         if (body->getCollisionShape())
 153         {
 154                 addCollisionObject(body, group, mask);
 155         }
 156 }
 157
 158 void btSimpleDynamicsWorld::debugDrawWorld()
 159 {
 160 }
 161
 162 void btSimpleDynamicsWorld::addAction(btActionInterface* action)
 163 {
 164 }
 165
 166 void btSimpleDynamicsWorld::removeAction(btActionInterface* action)
 167 {
 168 }
 169
 170 void btSimpleDynamicsWorld::updateAabbs()
 171 {
 172         btTransform predictedTrans;
 173         for (int i = 0; i < m_collisionObjects.size(); i++)
 174         {
 175                 btCollisionObject* colObj = m_collisionObjects[i];
 176                 btRigidBody* body = btRigidBody::upcast(colObj);
 177                 if (body)
 178                 {
 179                         if (body->isActive() && (!body->isStaticObject()))
 180                         {
 181                                 btVector3 minAabb, maxAabb;
 182                                 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
 183                                 btBroadphaseInterface* bp = getBroadphase();
 184                                 bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1);
 185                         }
 186                 }
 187         }
 188 }
 189
 190 void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 191 {
 192         btTransform predictedTrans;
 193         for (int i = 0; i < m_collisionObjects.size(); i++)
 194         {
 195                 btCollisionObject* colObj = m_collisionObjects[i];
 196                 btRigidBody* body = btRigidBody::upcast(colObj);
 197                 if (body)
 198                 {
 199                         if (body->isActive() && (!body->isStaticObject()))
 200                         {
 201                                 body->predictIntegratedTransform(timeStep, predictedTrans);
 202                                 body->proceedToTransform(predictedTrans);
 203                         }
 204                 }
 205         }
 206 }
 207
 208 void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 209 {
 210         for (int i = 0; i < m_collisionObjects.size(); i++)
 211         {
 212                 btCollisionObject* colObj = m_collisionObjects[i];
 213                 btRigidBody* body = btRigidBody::upcast(colObj);
 214                 if (body)
 215                 {
 216                         if (!body->isStaticObject())
 217                         {
 218                                 if (body->isActive())
 219                                 {
 220                                         body->applyGravity();
 221                                         body->integrateVelocities(timeStep);
 222                                         body->applyDamping(timeStep);
 223                                         body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
 224                                 }
 225                         }
 226                 }
 227         }
 228 }
 229
 230 void btSimpleDynamicsWorld::synchronizeMotionStates()
 231 {
 232         ///@todo: iterate over awake simulation islands!
 233         for (int i = 0; i < m_collisionObjects.size(); i++)
 234         {
 235                 btCollisionObject* colObj = m_collisionObjects[i];
 236                 btRigidBody* body = btRigidBody::upcast(colObj);
 237                 if (body && body->getMotionState())
 238                 {
 239                         if (body->getActivationState() != ISLAND_SLEEPING)
 240                         {
 241                                 body->getMotionState()->setWorldTransform(body->getWorldTransform());
 242                         }
 243                 }
 244         }
 245 }
 246
 247 void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 248 {
 249         if (m_ownsConstraintSolver)
 250         {
 251                 btAlignedFree(m_constraintSolver);
 252         }
 253         m_ownsConstraintSolver = false;
 254         m_constraintSolver = solver;
 255 }
 256
 257 btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
 258 {
 259         return m_constraintSolver;
 260 }