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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2009 Erwin Coumans  http://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 "btDiscreteDynamicsWorldMt.h"
  17
  18 //collision detection
  19 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  20 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
  21 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
  22 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
  23 #include "btSimulationIslandManagerMt.h"
  24 #include "LinearMath/btTransformUtil.h"
  25 #include "LinearMath/btQuickprof.h"
  26
  27 //rigidbody & constraints
  28 #include "BulletDynamics/Dynamics/btRigidBody.h"
  29 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
  30 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
  31 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  32 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
  33 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
  34 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
  35 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
  36 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
  37 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
  38 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
  39
  40 #include "LinearMath/btIDebugDraw.h"
  41 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  42
  43 #include "BulletDynamics/Dynamics/btActionInterface.h"
  44 #include "LinearMath/btQuickprof.h"
  45 #include "LinearMath/btMotionState.h"
  46
  47 #include "LinearMath/btSerializer.h"
  48
  49 ///
  50 /// btConstraintSolverPoolMt
  51 ///
  52
  53 btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
  54 {
  55         int i = 0;
  56 #if BT_THREADSAFE
  57         i = btGetCurrentThreadIndex() % m_solvers.size();
  58 #endif  // #if BT_THREADSAFE
  59         while (true)
  60         {
  61                 ThreadSolver& solver = m_solvers[i];
  62                 if (solver.mutex.tryLock())
  63                 {
  64                         return &solver;
  65                 }
  66                 // failed, try the next one
  67                 i = (i + 1) % m_solvers.size();
  68         }
  69         return NULL;
  70 }
  71
  72 void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers)
  73 {
  74         m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
  75         m_solvers.resize(numSolvers);
  76         for (int i = 0; i < numSolvers; ++i)
  77         {
  78                 m_solvers[i].solver = solvers[i];
  79         }
  80         if (numSolvers > 0)
  81         {
  82                 m_solverType = solvers[0]->getSolverType();
  83         }
  84 }
  85
  86 // create the solvers for me
  87 btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers)
  88 {
  89         btAlignedObjectArray<btConstraintSolver*> solvers;
  90         solvers.reserve(numSolvers);
  91         for (int i = 0; i < numSolvers; ++i)
  92         {
  93                 btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
  94                 solvers.push_back(solver);
  95         }
  96         init(&solvers[0], numSolvers);
  97 }
  98
  99 // pass in fully constructed solvers (destructor will delete them)
 100 btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers)
 101 {
 102         init(solvers, numSolvers);
 103 }
 104
 105 btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
 106 {
 107         // delete all solvers
 108         for (int i = 0; i < m_solvers.size(); ++i)
 109         {
 110                 ThreadSolver& solver = m_solvers[i];
 111                 delete solver.solver;
 112                 solver.solver = NULL;
 113         }
 114 }
 115
 116 ///solve a group of constraints
 117 btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies,
 118                                                                                           int numBodies,
 119                                                                                           btPersistentManifold** manifolds,
 120                                                                                           int numManifolds,
 121                                                                                           btTypedConstraint** constraints,
 122                                                                                           int numConstraints,
 123                                                                                           const btContactSolverInfo& info,
 124                                                                                           btIDebugDraw* debugDrawer,
 125                                                                                           btDispatcher* dispatcher)
 126 {
 127         ThreadSolver* ts = getAndLockThreadSolver();
 128         ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
 129         ts->mutex.unlock();
 130         return 0.0f;
 131 }
 132
 133 void btConstraintSolverPoolMt::reset()
 134 {
 135         for (int i = 0; i < m_solvers.size(); ++i)
 136         {
 137                 ThreadSolver& solver = m_solvers[i];
 138                 solver.mutex.lock();
 139                 solver.solver->reset();
 140                 solver.mutex.unlock();
 141         }
 142 }
 143
 144 ///
 145 /// btDiscreteDynamicsWorldMt
 146 ///
 147
 148 btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
 149                                                                                                          btBroadphaseInterface* pairCache,
 150                                                                                                          btConstraintSolverPoolMt* solverPool,
 151                                                                                                          btConstraintSolver* constraintSolverMt,
 152                                                                                                          btCollisionConfiguration* collisionConfiguration)
 153         : btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration)
 154 {
 155         if (m_ownsIslandManager)
 156         {
 157                 m_islandManager->~btSimulationIslandManager();
 158                 btAlignedFree(m_islandManager);
 159         }
 160         {
 161                 void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16);
 162                 btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
 163                 im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize);
 164                 m_islandManager = im;
 165         }
 166         m_constraintSolverMt = constraintSolverMt;
 167 }
 168
 169 btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
 170 {
 171 }
 172
 173 void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
 174 {
 175         BT_PROFILE("solveConstraints");
 176
 177         m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 178
 179         /// solve all the constraints for this island
 180         btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
 181         btSimulationIslandManagerMt::SolverParams solverParams;
 182         solverParams.m_solverPool = m_constraintSolver;
 183         solverParams.m_solverMt = m_constraintSolverMt;
 184         solverParams.m_solverInfo = &solverInfo;
 185         solverParams.m_debugDrawer = m_debugDrawer;
 186         solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
 187         im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams);
 188
 189         m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 190 }
 191
 192 struct UpdaterUnconstrainedMotion : public btIParallelForBody
 193 {
 194         btScalar timeStep;
 195         btRigidBody** rigidBodies;
 196
 197         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 198         {
 199                 for (int i = iBegin; i < iEnd; ++i)
 200                 {
 201                         btRigidBody* body = rigidBodies[i];
 202                         if (!body->isStaticOrKinematicObject())
 203                         {
 204                                 //don't integrate/update velocities here, it happens in the constraint solver
 205                                 body->applyDamping(timeStep);
 206                                 body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
 207                         }
 208                 }
 209         }
 210 };
 211
 212 void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep)
 213 {
 214         BT_PROFILE("predictUnconstraintMotion");
 215         if (m_nonStaticRigidBodies.size() > 0)
 216         {
 217                 UpdaterUnconstrainedMotion update;
 218                 update.timeStep = timeStep;
 219                 update.rigidBodies = &m_nonStaticRigidBodies[0];
 220                 int grainSize = 50;  // num of iterations per task for task scheduler
 221                 btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
 222         }
 223 }
 224
 225 void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep)
 226 {
 227         BT_PROFILE("createPredictiveContacts");
 228         releasePredictiveContacts();
 229         if (m_nonStaticRigidBodies.size() > 0)
 230         {
 231                 UpdaterCreatePredictiveContacts update;
 232                 update.world = this;
 233                 update.timeStep = timeStep;
 234                 update.rigidBodies = &m_nonStaticRigidBodies[0];
 235                 int grainSize = 50;  // num of iterations per task for task scheduler
 236                 btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
 237         }
 238 }
 239
 240 void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep)
 241 {
 242         BT_PROFILE("integrateTransforms");
 243         if (m_nonStaticRigidBodies.size() > 0)
 244         {
 245                 UpdaterIntegrateTransforms update;
 246                 update.world = this;
 247                 update.timeStep = timeStep;
 248                 update.rigidBodies = &m_nonStaticRigidBodies[0];
 249                 int grainSize = 50;  // num of iterations per task for task scheduler
 250                 btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
 251         }
 252 }
 253
 254 int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 255 {
 256         int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
 257         if (btITaskScheduler* scheduler = btGetTaskScheduler())
 258         {
 259                 // tell Bullet's threads to sleep, so other threads can run
 260                 scheduler->sleepWorkerThreadsHint();
 261         }
 262         return numSubSteps;
 263 }