diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Dynamics')
12 files changed, 1840 insertions, 2072 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h b/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h index e1fea3a49c..b5cac56cdc 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h @@ -26,21 +26,16 @@ class btCollisionWorld; class btActionInterface { protected: - static btRigidBody& getFixedBody(); - - -public: +public: virtual ~btActionInterface() { } - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0; + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) = 0; virtual void debugDraw(btIDebugDraw* debugDrawer) = 0; - }; -#endif //_BT_ACTION_INTERFACE_H - +#endif //_BT_ACTION_INTERFACE_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index b9944c138b..dfbbdb154f 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btDiscreteDynamicsWorld.h" //collision detection @@ -38,11 +37,9 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" - #include "BulletDynamics/Dynamics/btActionInterface.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" @@ -56,57 +53,52 @@ int startHit=2; int firstHit=startHit; #endif -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); return islandId; - } - class btSortConstraintOnIslandPredicate { - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } }; struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { - btContactSolverInfo* m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; + btContactSolverInfo* m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; btAlignedObjectArray<btCollisionObject*> m_bodies; btAlignedObjectArray<btPersistentManifold*> m_manifolds; btAlignedObjectArray<btTypedConstraint*> m_constraints; - InplaceSolverIslandCallback( - btConstraintSolver* solver, + btConstraintSolver* solver, btStackAlloc* stackAlloc, btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_sortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_sortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) { - } InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) @@ -116,34 +108,34 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal return *this; } - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) + SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) { btAssert(solverInfo); m_solverInfo = solverInfo; m_sortedConstraints = sortedConstraints; m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); } - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) { - if (islandId<0) + if (islandId < 0) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, &m_sortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + } + else { - //also add all non-contact constraints/joints for this island + //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; int numCurConstraints = 0; int i; //find the first constraint for this island - for (i=0;i<m_numConstraints;i++) + for (i = 0; i < m_numConstraints; i++) { if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) { @@ -152,7 +144,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal } } //count the number of constraints in this island - for (;i<m_numConstraints;i++) + for (; i < m_numConstraints; i++) { if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) { @@ -160,91 +152,87 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal } } - if (m_solverInfo->m_minimumSolverBatchSize<=1) + if (m_solverInfo->m_minimumSolverBatchSize <= 1) { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + } + else { - - for (i=0;i<numBodies;i++) + for (i = 0; i < numBodies; i++) m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) + for (i = 0; i < numManifolds; i++) m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) + for (i = 0; i < numCurConstraints; i++) m_constraints.push_back(startConstraint[i]); - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) + if ((m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) { processConstraints(); - } else + } + else { //printf("deferred\n"); } } } } - void processConstraints() + void processConstraints() { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - - m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher); + m_solver->solveGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); m_bodies.resize(0); m_manifolds.resize(0); m_constraints.resize(0); - } - }; - - -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_sortedConstraints (), -m_solverIslandCallback ( NULL ), -m_constraintSolver(constraintSolver), -m_gravity(0,-10,0), -m_localTime(0), -m_fixedTimeStep(0), -m_synchronizeAllMotionStates(false), -m_applySpeculativeContactRestitution(false), -m_profileTimings(0), -m_latencyMotionStateInterpolation(true) +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration), + m_sortedConstraints(), + m_solverIslandCallback(NULL), + m_constraintSolver(constraintSolver), + m_gravity(0, -10, 0), + m_localTime(0), + m_fixedTimeStep(0), + m_synchronizeAllMotionStates(false), + m_applySpeculativeContactRestitution(false), + m_profileTimings(0), + m_latencyMotionStateInterpolation(true) { if (!m_constraintSolver) { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); + void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver), 16); m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; m_ownsConstraintSolver = true; - } else + } + else { m_ownsConstraintSolver = false; } { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager), 16); m_islandManager = new (mem) btSimulationIslandManager(); } m_ownsIslandManager = true; { - void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16); - m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher); + void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback), 16); + m_solverIslandCallback = new (mem) InplaceSolverIslandCallback(m_constraintSolver, 0, dispatcher); } } - btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() { //only delete it when we created it if (m_ownsIslandManager) { m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); + btAlignedFree(m_islandManager); } if (m_solverIslandCallback) { @@ -253,18 +241,17 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() } if (m_ownsConstraintSolver) { - m_constraintSolver->~btConstraintSolver(); btAlignedFree(m_constraintSolver); } } -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) { -///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows -///to switch status _after_ adding kinematic objects to the world -///fix it for Bullet 3.x release - for (int i=0;i<m_collisionObjects.size();i++) + ///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows + ///to switch status _after_ adding kinematic objects to the world + ///fix it for Bullet 3.x release + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -277,10 +264,9 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) } } } - } -void btDiscreteDynamicsWorld::debugDrawWorld() +void btDiscreteDynamicsWorld::debugDrawWorld() { BT_PROFILE("debugDrawWorld"); @@ -290,43 +276,40 @@ void btDiscreteDynamicsWorld::debugDrawWorld() if (getDebugDrawer()) { int mode = getDebugDrawer()->getDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) { drawConstraints = true; } } - if(drawConstraints) + if (drawConstraints) { - for(int i = getNumConstraints()-1; i>=0 ;i--) + for (int i = getNumConstraints() - 1; i >= 0; i--) { btTypedConstraint* constraint = getConstraint(i); debugDrawConstraint(constraint); } } - - - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) { int i; if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) { - for (i=0;i<m_actions.size();i++) + for (i = 0; i < m_actions.size(); i++) { m_actions[i]->debugDraw(m_debugDrawer); } } } - if (getDebugDrawer()) - getDebugDrawer()->flushLines(); - + if (getDebugDrawer()) + getDebugDrawer()->flushLines(); } -void btDiscreteDynamicsWorld::clearForces() +void btDiscreteDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; //need to check if next line is ok @@ -336,10 +319,10 @@ void btDiscreteDynamicsWorld::clearForces() } ///apply gravity, call this once per timestep -void btDiscreteDynamicsWorld::applyGravity() +void btDiscreteDynamicsWorld::applyGravity() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body->isActive()) @@ -349,8 +332,7 @@ void btDiscreteDynamicsWorld::applyGravity() } } - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btAssert(body); @@ -363,32 +345,32 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), - (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), - interpolatedTransform); + body->getInterpolationLinearVelocity(), body->getInterpolationAngularVelocity(), + (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime * body->getHitFraction(), + interpolatedTransform); body->getMotionState()->setWorldTransform(interpolatedTransform); } } } - -void btDiscreteDynamicsWorld::synchronizeMotionStates() +void btDiscreteDynamicsWorld::synchronizeMotionStates() { -// BT_PROFILE("synchronizeMotionStates"); + // BT_PROFILE("synchronizeMotionStates"); if (m_synchronizeAllMotionStates) { //iterate over all collision objects - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); if (body) synchronizeSingleMotionState(body); } - } else + } + else { //iterate over all active rigid bodies - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body->isActive()) @@ -397,12 +379,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() } } - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { startProfiling(timeStep); - int numSimulationSubSteps = 0; if (maxSubSteps) @@ -412,10 +392,11 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, m_localTime += timeStep; if (m_localTime >= fixedTimeStep) { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); + numSimulationSubSteps = int(m_localTime / fixedTimeStep); m_localTime -= numSimulationSubSteps * fixedTimeStep; } - } else + } + else { //variable timestep fixedTimeStep = timeStep; @@ -425,7 +406,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, { numSimulationSubSteps = 0; maxSubSteps = 0; - } else + } + else { numSimulationSubSteps = 1; maxSubSteps = 1; @@ -435,28 +417,25 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, //process some debugging flags if (getDebugDrawer()) { - btIDebugDraw* debugDrawer = getDebugDrawer (); + btIDebugDraw* debugDrawer = getDebugDrawer(); gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } if (numSimulationSubSteps) { - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - saveKinematicState(fixedTimeStep*clampedSimulationSteps); + saveKinematicState(fixedTimeStep * clampedSimulationSteps); applyGravity(); - - - for (int i=0;i<clampedSimulationSteps;i++) + for (int i = 0; i < clampedSimulationSteps; i++) { internalSingleStepSimulation(fixedTimeStep); synchronizeMotionStates(); } - - } else + } + else { synchronizeMotionStates(); } @@ -465,17 +444,17 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, #ifndef BT_NO_PROFILE CProfileManager::Increment_Frame_Counter(); -#endif //BT_NO_PROFILE +#endif //BT_NO_PROFILE return numSimulationSubSteps; } -void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - BT_PROFILE("internalSingleStepSimulation"); - if(0 != m_internalPreTickCallback) { + if (0 != m_internalPreTickCallback) + { (*m_internalPreTickCallback)(this, timeStep); } @@ -488,19 +467,15 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); - - createPredictiveContacts(timeStep); + createPredictiveContacts(timeStep); ///perform collision detection performDiscreteCollisionDetection(); calculateSimulationIslands(); - getSolverInfo().m_timeStep = timeStep; - - ///solve contact and other joint constraints solveConstraints(getSolverInfo()); @@ -513,37 +488,38 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///update vehicle simulation updateActions(timeStep); - updateActivationState( timeStep ); + updateActivationState(timeStep); - if(0 != m_internalTickCallback) { + if (0 != m_internalTickCallback) + { (*m_internalTickCallback)(this, timeStep); } } -void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) +void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (body->isActive() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(gravity); } } } -btVector3 btDiscreteDynamicsWorld::getGravity () const +btVector3 btDiscreteDynamicsWorld::getGravity() const { return m_gravity; } -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) { - btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); + btCollisionWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask); } -void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btRigidBody* body = btRigidBody::upcast(collisionObject); if (body) @@ -552,16 +528,15 @@ void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collision btCollisionWorld::removeCollisionObject(collisionObject); } -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { m_nonStaticRigidBodies.remove(body); btCollisionWorld::removeCollisionObject(body); } - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) { - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } @@ -571,22 +546,23 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) if (!body->isStaticObject()) { m_nonStaticRigidBodies.push_back(body); - } else + } + else { body->setActivationState(ISLAND_SLEEPING); } bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); - int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + addCollisionObject(body, collisionFilterGroup, collisionFilterMask); } } -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } @@ -597,31 +573,29 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mas { m_nonStaticRigidBodies.push_back(body); } - else + else { body->setActivationState(ISLAND_SLEEPING); } - addCollisionObject(body,group,mask); + addCollisionObject(body, group, mask); } } - -void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) +void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) { BT_PROFILE("updateActions"); - for ( int i=0;i<m_actions.size();i++) + for (int i = 0; i < m_actions.size(); i++) { - m_actions[i]->updateAction( this, timeStep); + m_actions[i]->updateAction(this, timeStep); } } - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body) @@ -633,32 +607,33 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) if (body->isStaticOrKinematicObject()) { body->setActivationState(ISLAND_SLEEPING); - } else + } + else { if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); + body->setActivationState(WANTS_DEACTIVATION); if (body->getActivationState() == ISLAND_SLEEPING) { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); + body->setAngularVelocity(btVector3(0, 0, 0)); + body->setLinearVelocity(btVector3(0, 0, 0)); } - } - } else + } + else { if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); + body->setActivationState(ACTIVE_TAG); } } } } -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies) { m_constraints.push_back(constraint); - //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) - btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB()); - + //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) + btAssert(&constraint->getRigidBodyA() != &constraint->getRigidBodyB()); + if (disableCollisionsBetweenLinkedBodies) { constraint->getRigidBodyA().addConstraintRef(constraint); @@ -666,105 +641,98 @@ void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool d } } -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) { m_constraints.remove(constraint); constraint->getRigidBodyA().removeConstraintRef(constraint); constraint->getRigidBodyB().removeConstraintRef(constraint); } -void btDiscreteDynamicsWorld::addAction(btActionInterface* action) +void btDiscreteDynamicsWorld::addAction(btActionInterface* action) { m_actions.push_back(action); } -void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) +void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) { m_actions.remove(action); } - -void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) +void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) { addAction(vehicle); } -void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) +void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) { removeAction(vehicle); } -void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) +void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) { addAction(character); } -void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) +void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) { removeAction(character); } - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); - m_sortedConstraints.resize( m_constraints.size()); + m_sortedConstraints.resize(m_constraints.size()); int i; - for (i=0;i<getNumConstraints();i++) + for (i = 0; i < getNumConstraints(); i++) { m_sortedConstraints[i] = m_constraints[i]; } -// btAssert(0); - - + // btAssert(0); m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer()); + m_solverIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverIslandCallback); m_solverIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); } - -void btDiscreteDynamicsWorld::calculateSimulationIslands() +void btDiscreteDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher()); - { - //merge islands based on speculative contact manifolds too - for (int i=0;i<this->m_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; + { + //merge islands based on speculative contact manifolds too + for (int i = 0; i < this->m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); + } + } + } { int i; int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) + for (i = 0; i < numConstraints; i++) { btTypedConstraint* constraint = m_constraints[i]; if (constraint->isEnabled()) @@ -775,7 +743,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); } } } @@ -783,51 +751,44 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - - } - - - class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { public: - btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btCollisionWorld::ClosestConvexResultCallback(fromA,toA), - m_me(me), - m_allowedPenetration(0.0f), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + btClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA, toA), + m_me(me), + m_allowedPenetration(0.0f), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) return 1.0f; //ignore result if there is no contact response - if(!convexResult.m_hitCollisionObject->hasContactResponse()) + if (!convexResult.m_hitCollisionObject->hasContactResponse()) return 1.0f; - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = m_convexToWorld - m_convexFromWorld; + linVelB = btVector3(0, 0, 0); //toB.getOrigin()-fromB.getOrigin(); - btVector3 relativeVelocity = (linVelA-linVelB); + btVector3 relativeVelocity = (linVelA - linVelB); //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity) >= -m_allowedPenetration) return 1.f; - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); } virtual bool needsCollision(btBroadphaseProxy* proxy0) const @@ -840,13 +801,13 @@ public: if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; - if(!m_dispatcher->needsCollision(m_me, otherObj)) + if (!m_dispatcher->needsCollision(m_me, otherObj)) return false; //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 - if (m_dispatcher->needsResponse(m_me,otherObj)) + if (m_dispatcher->needsResponse(m_me, otherObj)) { #if 0 ///don't do CCD when there are already contact points (touching contact/penetration) @@ -872,28 +833,24 @@ public: return false; } - - }; ///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; - +int gNumClampedCcdMotions = 0; -void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep) +void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { btTransform predictedTrans; - for ( int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { btRigidBody* body = bodies[i]; body->setHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -905,60 +862,55 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: - - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher) { } - virtual bool needsCollision(btBroadphaseProxy* proxy0) const + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; - StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #else - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - - btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; + btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction; btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); - - btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); - btMutexLock( &m_predictiveManifoldsMutex ); + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject); + btMutexLock(&m_predictiveManifoldsMutex); m_predictiveManifolds.push_back(manifold); - btMutexUnlock( &m_predictiveManifoldsMutex ); + btMutexUnlock(&m_predictiveManifoldsMutex); - btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; - btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; + btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec; + btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB; - btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance); + btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance); bool isPredictive = true; int index = manifold->addManifoldPoint(newPoint, isPredictive); btManifoldPoint& pt = manifold->getContactPoint(index); pt.m_combinedRestitution = 0; - pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body,sweepResults.m_hitCollisionObject); + pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject); pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); pt.m_positionWorldOnB = worldPointB; - } } } @@ -968,42 +920,39 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo void btDiscreteDynamicsWorld::releasePredictiveContacts() { - BT_PROFILE( "release predictive contact manifolds" ); - - for ( int i = 0; i < m_predictiveManifolds.size(); i++ ) - { - btPersistentManifold* manifold = m_predictiveManifolds[ i ]; - this->m_dispatcher1->releaseManifold( manifold ); - } - m_predictiveManifolds.clear(); + BT_PROFILE("release predictive contact manifolds"); + + for (int i = 0; i < m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + this->m_dispatcher1->releaseManifold(manifold); + } + m_predictiveManifolds.clear(); } void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) { BT_PROFILE("createPredictiveContacts"); - releasePredictiveContacts(); - if (m_nonStaticRigidBodies.size() > 0) - { - createPredictiveContactsInternal( &m_nonStaticRigidBodies[ 0 ], m_nonStaticRigidBodies.size(), timeStep ); - } + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } } -void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ) +void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { btTransform predictedTrans; - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { btRigidBody* body = bodies[i]; body->setHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - + btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -1015,43 +964,40 @@ void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: - - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher) { } - virtual bool needsCollision(btBroadphaseProxy* proxy0) const + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; - StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #else - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - //printf("clamped integration to hit fraction = %f\n",fraction); body->setHitFraction(sweepResults.m_closestHitFraction); - body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans); body->setHitFraction(0.f); - body->proceedToTransform( predictedTrans); + body->proceedToTransform(predictedTrans); #if 0 btVector3 linVel = body->getLinearVelocity(); @@ -1078,50 +1024,45 @@ void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, //btScalar depth = 0.f; //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); - #endif - continue; + continue; } } } - - body->proceedToTransform( predictedTrans); - + body->proceedToTransform(predictedTrans); } - } - } void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); - if (m_nonStaticRigidBodies.size() > 0) - { - integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); - } + if (m_nonStaticRigidBodies.size() > 0) + { + integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } - ///this should probably be switched on by default, but it is not well tested yet + ///this should probably be switched on by default, but it is not well tested yet if (m_applySpeculativeContactRestitution) { BT_PROFILE("apply speculative contact restitution"); - for (int i=0;i<m_predictiveManifolds.size();i++) + for (int i = 0; i < m_predictiveManifolds.size(); i++) { btPersistentManifold* manifold = m_predictiveManifolds[i]; btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0()); btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1()); - for (int p=0;p<manifold->getNumContacts();p++) + for (int p = 0; p < manifold->getNumContacts(); p++) { const btManifoldPoint& pt = manifold->getContactPoint(p); btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1); - if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f) + if (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f) //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f) { - btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution; + btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution; const btVector3& pos1 = pt.getPositionWorldOnA(); const btVector3& pos2 = pt.getPositionWorldOnB(); @@ -1130,23 +1071,19 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); if (body0) - body0->applyImpulse(imp,rel_pos0); + body0->applyImpulse(imp, rel_pos0); if (body1) - body1->applyImpulse(-imp,rel_pos1); + body1->applyImpulse(-imp, rel_pos1); } } } } } - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (!body->isStaticOrKinematicObject()) @@ -1155,179 +1092,171 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); } } } - -void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) +void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) { (void)timeStep; #ifndef BT_NO_PROFILE CProfileManager::Reset(); -#endif //BT_NO_PROFILE - +#endif //BT_NO_PROFILE } - - - - - void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) { bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; btScalar dbgDrawSize = constraint->getDbgDrawSize(); - if(dbgDrawSize <= btScalar(0.f)) + if (dbgDrawSize <= btScalar(0.f)) { return; } - switch(constraint->getConstraintType()) + switch (constraint->getConstraintType()) { case POINT2POINT_CONSTRAINT_TYPE: + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; + case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if (minAng == maxAng) { - btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; - btTransform tr; - tr.setIdentity(); - btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame - pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + break; } - break; - case HINGE_CONSTRAINT_TYPE: + bool drawSect = true; + if (!pHinge->hasLimit()) { - btHingeConstraint* pHinge = (btHingeConstraint*)constraint; - btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - btScalar minAng = pHinge->getLowerLimit(); - btScalar maxAng = pHinge->getUpperLimit(); - if(minAng == maxAng) - { - break; - } - bool drawSect = true; - if(!pHinge->hasLimit()) - { - minAng = btScalar(0.f); - maxAng = SIMD_2_PI; - drawSect = false; - } - if(drawLimits) - { - btVector3& center = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); - } + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; } - break; + if (drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect); + } + } + break; case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; - btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8 * 4; + btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i = 0; i < nSegments; i++) { - //const btScalar length = btScalar(5); - const btScalar length = dbgDrawSize; - static int nSegments = 8*4; - btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); - btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); - pPrev = tr * pPrev; - for (int i=0; i<nSegments; i++) - { - fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments); - btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); - pCur = tr * pCur; - getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); - - if (i%(nSegments/8) == 0) - getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); + fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments); + btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0)); - pPrev = pCur; - } - btScalar tws = pCT->getTwistSpan(); - btScalar twa = pCT->getTwistAngle(); - bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); - if(useFrameB) - { - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - } - else - { - tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - } - btVector3 pivot = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis1 = tr.getBasis().getColumn(1); - getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + if (i % (nSegments / 8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0, 0, 0)); + pPrev = pCur; + } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if (useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true); } - break; + } + break; case D6_SPRING_CONSTRAINT_TYPE: case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; - btTransform tr = p6DOF->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; + ref[1] = -sz * axis[0] + cz * axis[1]; + ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; tr = p6DOF->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if (minFi > maxFi) { - tr = p6DOF->getCalculatedTransformA(); - const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); - btVector3 up = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; - btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - btScalar cy = btCos(ay); - btScalar sy = btSin(ay); - btScalar cz = btCos(az); - btScalar sz = btSin(az); - btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; - tr = p6DOF->getCalculatedTransformB(); - btVector3 normal = -tr.getBasis().getColumn(0); - btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; - btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - if(minFi > maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); - } - else if(minFi < maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); - } - tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false); } + else if (minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0)); } - break; + } + break; ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage case D6_SPRING_2_CONSTRAINT_TYPE: { @@ -1359,9 +1288,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btScalar cz = btCos(az); btScalar sz = btSin(az); btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; + ref[1] = -sz * axis[0] + cz * axis[1]; + ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; tr = p6DOF->getCalculatedTransformB(); btVector3 normal = -tr.getBasis().getColumn(0); btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; @@ -1383,42 +1312,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) break; } case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btSliderConstraint* pSlider = (btSliderConstraint*)constraint; - btTransform tr = pSlider->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pSlider->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); - btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); - btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); - getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis = tr.getBasis().getColumn(1); - btScalar a_min = pSlider->getLowerAngLimit(); - btScalar a_max = pSlider->getUpperAngLimit(); - const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); - } + btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true); } - break; - default : + } + break; + default: break; } return; } - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { - btAlignedFree( m_constraintSolver); + btAlignedFree(m_constraintSolver); } m_ownsConstraintSolver = false; m_constraintSolver = solver; @@ -1430,8 +1355,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() return m_constraintSolver; } - -int btDiscreteDynamicsWorld::getNumConstraints() const +int btDiscreteDynamicsWorld::getNumConstraints() const { return int(m_constraints.size()); } @@ -1444,93 +1368,87 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const return m_constraints[index]; } - - -void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) +void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) { int i; //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) { int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj); } } - for (i=0;i<m_constraints.size();i++) + for (i = 0; i < m_constraints.size(); i++) { btTypedConstraint* constraint = m_constraints[i]; int size = constraint->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(size,1); - const char* structType = constraint->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint); + btChunk* chunk = serializer->allocate(size, 1); + const char* structType = constraint->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint); } } - - - -void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) +void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) { #ifdef BT_USE_DOUBLE_PRECISION - int len = sizeof(btDynamicsWorldDoubleData); - btChunk* chunk = serializer->allocate(len,1); - btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; -#else//BT_USE_DOUBLE_PRECISION - int len = sizeof(btDynamicsWorldFloatData); - btChunk* chunk = serializer->allocate(len,1); - btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; -#endif//BT_USE_DOUBLE_PRECISION - - memset(worldInfo ,0x00,len); - - m_gravity.serialize(worldInfo->m_gravity); - worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; - worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; - worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; - worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; - - worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; - worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; - worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; - worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; - - worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; - worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; - worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; - worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; - - worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; - worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; - worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; - worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; - - worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; - worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; - worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; - worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; - - worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; - - // Fill padding with zeros to appease msan. - memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding)); + int len = sizeof(btDynamicsWorldDoubleData); + btChunk* chunk = serializer->allocate(len, 1); + btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; +#else //BT_USE_DOUBLE_PRECISION + int len = sizeof(btDynamicsWorldFloatData); + btChunk* chunk = serializer->allocate(len, 1); + btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; +#endif //BT_USE_DOUBLE_PRECISION + + memset(worldInfo, 0x00, len); + + m_gravity.serialize(worldInfo->m_gravity); + worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; + worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; + worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; + worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; + + worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; + worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; + worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; + worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; + + worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; + worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; + worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; + worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; + + worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; + worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; + worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; + worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; + + worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; + worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; + worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; + worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; + + worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; + + // Fill padding with zeros to appease msan. + memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding)); #ifdef BT_USE_DOUBLE_PRECISION - const char* structType = "btDynamicsWorldDoubleData"; -#else//BT_USE_DOUBLE_PRECISION - const char* structType = "btDynamicsWorldFloatData"; -#endif//BT_USE_DOUBLE_PRECISION - serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo); + const char* structType = "btDynamicsWorldDoubleData"; +#else //BT_USE_DOUBLE_PRECISION + const char* structType = "btDynamicsWorldFloatData"; +#endif //BT_USE_DOUBLE_PRECISION + serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo); } -void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) +void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); serializeDynamicsWorldInfo(serializer); @@ -1543,4 +1461,3 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) serializer->finishSerialization(); } - diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index b0d19f48a3..7fe9619213 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_DYNAMICS_WORLD_H #define BT_DISCRETE_DYNAMICS_WORLD_H @@ -32,159 +31,153 @@ struct InplaceSolverIslandCallback; #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btThreads.h" - ///btDiscreteDynamicsWorld provides discrete rigid body simulation ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld +ATTRIBUTE_ALIGNED16(class) +btDiscreteDynamicsWorld : public btDynamicsWorld { protected: - - btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints; - InplaceSolverIslandCallback* m_solverIslandCallback; + btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints; + InplaceSolverIslandCallback* m_solverIslandCallback; - btConstraintSolver* m_constraintSolver; + btConstraintSolver* m_constraintSolver; - btSimulationIslandManager* m_islandManager; + btSimulationIslandManager* m_islandManager; btAlignedObjectArray<btTypedConstraint*> m_constraints; btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies; - btVector3 m_gravity; + btVector3 m_gravity; //for variable timesteps - btScalar m_localTime; - btScalar m_fixedTimeStep; + btScalar m_localTime; + btScalar m_fixedTimeStep; //for variable timesteps - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - bool m_synchronizeAllMotionStates; - bool m_applySpeculativeContactRestitution; + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + bool m_synchronizeAllMotionStates; + bool m_applySpeculativeContactRestitution; - btAlignedObjectArray<btActionInterface*> m_actions; - - int m_profileTimings; + btAlignedObjectArray<btActionInterface*> m_actions; - bool m_latencyMotionStateInterpolation; + int m_profileTimings; - btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds; - btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts + bool m_latencyMotionStateInterpolation; - virtual void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel - virtual void integrateTransforms(btScalar timeStep); - - virtual void calculateSimulationIslands(); + btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds; + btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void updateActivationState(btScalar timeStep); + virtual void predictUnconstraintMotion(btScalar timeStep); - void updateActions(btScalar timeStep); + void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel + virtual void integrateTransforms(btScalar timeStep); - void startProfiling(btScalar timeStep); + virtual void calculateSimulationIslands(); - virtual void internalSingleStepSimulation( btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo & solverInfo); - void releasePredictiveContacts(); - void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel - virtual void createPredictiveContacts(btScalar timeStep); + virtual void updateActivationState(btScalar timeStep); - virtual void saveKinematicState(btScalar timeStep); + void updateActions(btScalar timeStep); - void serializeRigidBodies(btSerializer* serializer); + void startProfiling(btScalar timeStep); - void serializeDynamicsWorldInfo(btSerializer* serializer); + virtual void internalSingleStepSimulation(btScalar timeStep); -public: + void releasePredictiveContacts(); + void createPredictiveContactsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel + virtual void createPredictiveContacts(btScalar timeStep); + virtual void saveKinematicState(btScalar timeStep); + void serializeRigidBodies(btSerializer * serializer); + + void serializeDynamicsWorldInfo(btSerializer * serializer); + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btDiscreteDynamicsWorld(btDispatcher * dispatcher, btBroadphaseInterface * pairCache, btConstraintSolver * constraintSolver, btCollisionConfiguration * collisionConfiguration); virtual ~btDiscreteDynamicsWorld(); ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); - - virtual void synchronizeMotionStates(); + virtual void synchronizeMotionStates(); ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); + void synchronizeSingleMotionState(btRigidBody * body); + + virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false); - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual void removeConstraint(btTypedConstraint * constraint); - virtual void removeConstraint(btTypedConstraint* constraint); + virtual void addAction(btActionInterface*); - virtual void addAction(btActionInterface*); + virtual void removeAction(btActionInterface*); - virtual void removeAction(btActionInterface*); - - btSimulationIslandManager* getSimulationIslandManager() + btSimulationIslandManager* getSimulationIslandManager() { return m_islandManager; } - const btSimulationIslandManager* getSimulationIslandManager() const + const btSimulationIslandManager* getSimulationIslandManager() const { return m_islandManager; } - btCollisionWorld* getCollisionWorld() + btCollisionWorld* getCollisionWorld() { return this; } - virtual void setGravity(const btVector3& gravity); + virtual void setGravity(const btVector3& gravity); - virtual btVector3 getGravity () const; + virtual btVector3 getGravity() const; - virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + virtual void addCollisionObject(btCollisionObject * collisionObject, int collisionFilterGroup = btBroadphaseProxy::StaticFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - virtual void addRigidBody(btRigidBody* body); + virtual void addRigidBody(btRigidBody * body); - virtual void addRigidBody(btRigidBody* body, int group, int mask); + virtual void addRigidBody(btRigidBody * body, int group, int mask); - virtual void removeRigidBody(btRigidBody* body); + virtual void removeRigidBody(btRigidBody * body); ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - + virtual void removeCollisionObject(btCollisionObject * collisionObject); - virtual void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawConstraint(btTypedConstraint * constraint); - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); - virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver * solver); virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - virtual btTypedConstraint* getConstraint(int index) ; + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index); virtual const btTypedConstraint* getConstraint(int index) const; - - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { return BT_DISCRETE_DYNAMICS_WORLD; } - + ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. - virtual void clearForces(); + virtual void clearForces(); ///apply gravity, call this once per timestep - virtual void applyGravity(); + virtual void applyGravity(); - virtual void setNumTasks(int numTasks) + virtual void setNumTasks(int numTasks) { - (void) numTasks; + (void)numTasks; } ///obsolete, use updateActions instead @@ -194,15 +187,15 @@ public: } ///obsolete, use addAction instead - virtual void addVehicle(btActionInterface* vehicle); + virtual void addVehicle(btActionInterface * vehicle); ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle); + virtual void removeVehicle(btActionInterface * vehicle); ///obsolete, use addAction instead - virtual void addCharacter(btActionInterface* character); + virtual void addCharacter(btActionInterface * character); ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character); + virtual void removeCharacter(btActionInterface * character); - void setSynchronizeAllMotionStates(bool synchronizeAll) + void setSynchronizeAllMotionStates(bool synchronizeAll) { m_synchronizeAllMotionStates = synchronizeAll; } @@ -215,18 +208,18 @@ public: { m_applySpeculativeContactRestitution = enable; } - + bool getApplySpeculativeContactRestitution() const { return m_applySpeculativeContactRestitution; } ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo) - virtual void serialize(btSerializer* serializer); + virtual void serialize(btSerializer * serializer); ///Interpolate motion state between previous and current transform, instead of current and next transform. ///This can relieve discontinuities in the rendering, due to penetrations - void setLatencyMotionStateInterpolation(bool latencyInterpolation ) + void setLatencyMotionStateInterpolation(bool latencyInterpolation) { m_latencyMotionStateInterpolation = latencyInterpolation; } @@ -236,4 +229,4 @@ public: } }; -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp index d705bf2381..8207b47135 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btDiscreteDynamicsWorldMt.h" //collision detection @@ -38,148 +37,139 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" - #include "BulletDynamics/Dynamics/btActionInterface.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" #include "LinearMath/btSerializer.h" - - /// /// btConstraintSolverPoolMt /// btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver() { - int i = 0; + int i = 0; #if BT_THREADSAFE - i = btGetCurrentThreadIndex() % m_solvers.size(); -#endif // #if BT_THREADSAFE - while ( true ) - { - ThreadSolver& solver = m_solvers[ i ]; - if ( solver.mutex.tryLock() ) - { - return &solver; - } - // failed, try the next one - i = ( i + 1 ) % m_solvers.size(); - } - return NULL; + i = btGetCurrentThreadIndex() % m_solvers.size(); +#endif // #if BT_THREADSAFE + while (true) + { + ThreadSolver& solver = m_solvers[i]; + if (solver.mutex.tryLock()) + { + return &solver; + } + // failed, try the next one + i = (i + 1) % m_solvers.size(); + } + return NULL; } -void btConstraintSolverPoolMt::init( btConstraintSolver** solvers, int numSolvers ) +void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers) { - m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER; - m_solvers.resize( numSolvers ); - for ( int i = 0; i < numSolvers; ++i ) - { - m_solvers[ i ].solver = solvers[ i ]; - } - if ( numSolvers > 0 ) - { - m_solverType = solvers[ 0 ]->getSolverType(); - } + m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER; + m_solvers.resize(numSolvers); + for (int i = 0; i < numSolvers; ++i) + { + m_solvers[i].solver = solvers[i]; + } + if (numSolvers > 0) + { + m_solverType = solvers[0]->getSolverType(); + } } // create the solvers for me -btConstraintSolverPoolMt::btConstraintSolverPoolMt( int numSolvers ) +btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers) { - btAlignedObjectArray<btConstraintSolver*> solvers; - solvers.reserve( numSolvers ); - for ( int i = 0; i < numSolvers; ++i ) - { - btConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); - solvers.push_back( solver ); - } - init( &solvers[ 0 ], numSolvers ); + btAlignedObjectArray<btConstraintSolver*> solvers; + solvers.reserve(numSolvers); + for (int i = 0; i < numSolvers; ++i) + { + btConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); + solvers.push_back(solver); + } + init(&solvers[0], numSolvers); } // pass in fully constructed solvers (destructor will delete them) -btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers ) +btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers) { - init( solvers, numSolvers ); + init(solvers, numSolvers); } btConstraintSolverPoolMt::~btConstraintSolverPoolMt() { - // delete all solvers - for ( int i = 0; i < m_solvers.size(); ++i ) - { - ThreadSolver& solver = m_solvers[ i ]; - delete solver.solver; - solver.solver = NULL; - } + // delete all solvers + for (int i = 0; i < m_solvers.size(); ++i) + { + ThreadSolver& solver = m_solvers[i]; + delete solver.solver; + solver.solver = NULL; + } } ///solve a group of constraints -btScalar btConstraintSolverPoolMt::solveGroup( btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifolds, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - const btContactSolverInfo& info, - btIDebugDraw* debugDrawer, - btDispatcher* dispatcher -) +btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifolds, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) { - ThreadSolver* ts = getAndLockThreadSolver(); - ts->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher ); - ts->mutex.unlock(); - return 0.0f; + ThreadSolver* ts = getAndLockThreadSolver(); + ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); + ts->mutex.unlock(); + return 0.0f; } void btConstraintSolverPoolMt::reset() { - for ( int i = 0; i < m_solvers.size(); ++i ) - { - ThreadSolver& solver = m_solvers[ i ]; - solver.mutex.lock(); - solver.solver->reset(); - solver.mutex.unlock(); - } + for (int i = 0; i < m_solvers.size(); ++i) + { + ThreadSolver& solver = m_solvers[i]; + solver.mutex.lock(); + solver.solver->reset(); + solver.mutex.unlock(); + } } - /// /// btDiscreteDynamicsWorldMt /// btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, - btBroadphaseInterface* pairCache, - btConstraintSolverPoolMt* constraintSolver, - btConstraintSolver* constraintSolverMt, - btCollisionConfiguration* collisionConfiguration -) -: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) + btBroadphaseInterface* pairCache, + btConstraintSolverPoolMt* solverPool, + btConstraintSolver* constraintSolverMt, + btCollisionConfiguration* collisionConfiguration) + : btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration) { if (m_ownsIslandManager) { m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); + btAlignedFree(m_islandManager); } { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16); + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16); btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt(); - im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize ); - m_islandManager = im; + im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize); + m_islandManager = im; } - m_constraintSolverMt = constraintSolverMt; + m_constraintSolverMt = constraintSolverMt; } - btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt() { } - void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); @@ -187,92 +177,87 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager); - btSimulationIslandManagerMt::SolverParams solverParams; - solverParams.m_solverPool = m_constraintSolver; - solverParams.m_solverMt = m_constraintSolverMt; - solverParams.m_solverInfo = &solverInfo; - solverParams.m_debugDrawer = m_debugDrawer; - solverParams.m_dispatcher = getCollisionWorld()->getDispatcher(); - im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams ); + btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager); + btSimulationIslandManagerMt::SolverParams solverParams; + solverParams.m_solverPool = m_constraintSolver; + solverParams.m_solverMt = m_constraintSolverMt; + solverParams.m_solverInfo = &solverInfo; + solverParams.m_debugDrawer = m_debugDrawer; + solverParams.m_dispatcher = getCollisionWorld()->getDispatcher(); + im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); } - struct UpdaterUnconstrainedMotion : public btIParallelForBody { - btScalar timeStep; - btRigidBody** rigidBodies; + btScalar timeStep; + btRigidBody** rigidBodies; - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - for ( int i = iBegin; i < iEnd; ++i ) - { - btRigidBody* body = rigidBodies[ i ]; - if ( !body->isStaticOrKinematicObject() ) - { - //don't integrate/update velocities here, it happens in the constraint solver - body->applyDamping( timeStep ); - body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() ); - } - } - } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + for (int i = iBegin; i < iEnd; ++i) + { + btRigidBody* body = rigidBodies[i]; + if (!body->isStaticOrKinematicObject()) + { + //don't integrate/update velocities here, it happens in the constraint solver + body->applyDamping(timeStep); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); + } + } + } }; - -void btDiscreteDynamicsWorldMt::predictUnconstraintMotion( btScalar timeStep ) +void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep) { - BT_PROFILE( "predictUnconstraintMotion" ); - if ( m_nonStaticRigidBodies.size() > 0 ) - { - UpdaterUnconstrainedMotion update; - update.timeStep = timeStep; - update.rigidBodies = &m_nonStaticRigidBodies[ 0 ]; - int grainSize = 50; // num of iterations per task for task scheduler - btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update ); - } + BT_PROFILE("predictUnconstraintMotion"); + if (m_nonStaticRigidBodies.size() > 0) + { + UpdaterUnconstrainedMotion update; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[0]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update); + } } - -void btDiscreteDynamicsWorldMt::createPredictiveContacts( btScalar timeStep ) +void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep) { - BT_PROFILE( "createPredictiveContacts" ); - releasePredictiveContacts(); - if ( m_nonStaticRigidBodies.size() > 0 ) - { - UpdaterCreatePredictiveContacts update; - update.world = this; - update.timeStep = timeStep; - update.rigidBodies = &m_nonStaticRigidBodies[ 0 ]; - int grainSize = 50; // num of iterations per task for task scheduler - btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update ); - } + BT_PROFILE("createPredictiveContacts"); + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + UpdaterCreatePredictiveContacts update; + update.world = this; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[0]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update); + } } - -void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep ) +void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep) { - BT_PROFILE( "integrateTransforms" ); - if ( m_nonStaticRigidBodies.size() > 0 ) - { - UpdaterIntegrateTransforms update; - update.world = this; - update.timeStep = timeStep; - update.rigidBodies = &m_nonStaticRigidBodies[ 0 ]; - int grainSize = 50; // num of iterations per task for task scheduler - btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update ); - } + BT_PROFILE("integrateTransforms"); + if (m_nonStaticRigidBodies.size() > 0) + { + UpdaterIntegrateTransforms update; + update.world = this; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[0]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update); + } } - -int btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) +int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { - int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep); - if (btITaskScheduler* scheduler = btGetTaskScheduler()) - { - // tell Bullet's threads to sleep, so other threads can run - scheduler->sleepWorkerThreadsHint(); - } - return numSubSteps; + int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep); + if (btITaskScheduler* scheduler = btGetTaskScheduler()) + { + // tell Bullet's threads to sleep, so other threads can run + scheduler->sleepWorkerThreadsHint(); + } + return numSubSteps; } diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h index 667fe5800e..dccf35d7a7 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_DYNAMICS_WORLD_MT_H #define BT_DISCRETE_DYNAMICS_WORLD_MT_H @@ -21,7 +20,6 @@ subject to the following restrictions: #include "btSimulationIslandManagerMt.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" - /// /// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them. /// @@ -34,46 +32,43 @@ subject to the following restrictions: class btConstraintSolverPoolMt : public btConstraintSolver { public: - // create the solvers for me - explicit btConstraintSolverPoolMt( int numSolvers ); + // create the solvers for me + explicit btConstraintSolverPoolMt(int numSolvers); - // pass in fully constructed solvers (destructor will delete them) - btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers ); + // pass in fully constructed solvers (destructor will delete them) + btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers); - virtual ~btConstraintSolverPoolMt(); + virtual ~btConstraintSolverPoolMt(); - ///solve a group of constraints - virtual btScalar solveGroup( btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifolds, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - const btContactSolverInfo& info, - btIDebugDraw* debugDrawer, - btDispatcher* dispatcher - ) BT_OVERRIDE; + ///solve a group of constraints + virtual btScalar solveGroup(btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifolds, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) BT_OVERRIDE; - virtual void reset() BT_OVERRIDE; - virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; } + virtual void reset() BT_OVERRIDE; + virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; } private: - const static size_t kCacheLineSize = 128; - struct ThreadSolver - { - btConstraintSolver* solver; - btSpinMutex mutex; - char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ]; // keep mutexes from sharing a cache line - }; - btAlignedObjectArray<ThreadSolver> m_solvers; - btConstraintSolverType m_solverType; - - ThreadSolver* getAndLockThreadSolver(); - void init( btConstraintSolver** solvers, int numSolvers ); + const static size_t kCacheLineSize = 128; + struct ThreadSolver + { + btConstraintSolver* solver; + btSpinMutex mutex; + char _cachelinePadding[kCacheLineSize - sizeof(btSpinMutex) - sizeof(void*)]; // keep mutexes from sharing a cache line + }; + btAlignedObjectArray<ThreadSolver> m_solvers; + btConstraintSolverType m_solverType; + + ThreadSolver* getAndLockThreadSolver(); + void init(btConstraintSolver** solvers, int numSolvers); }; - - /// /// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support /// solving simulation islands on multiple threads. @@ -84,53 +79,53 @@ private: /// - integrateTransforms /// - createPredictiveContacts /// -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld +ATTRIBUTE_ALIGNED16(class) +btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld { protected: - btConstraintSolver* m_constraintSolverMt; - - virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE; - - virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE; - - struct UpdaterCreatePredictiveContacts : public btIParallelForBody - { - btScalar timeStep; - btRigidBody** rigidBodies; - btDiscreteDynamicsWorldMt* world; - - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep ); - } - }; - virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE; - - struct UpdaterIntegrateTransforms : public btIParallelForBody - { - btScalar timeStep; - btRigidBody** rigidBodies; - btDiscreteDynamicsWorldMt* world; - - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep ); - } - }; - virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE; + btConstraintSolver* m_constraintSolverMt; + + virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE; + + virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE; + + struct UpdaterCreatePredictiveContacts : public btIParallelForBody + { + btScalar timeStep; + btRigidBody** rigidBodies; + btDiscreteDynamicsWorldMt* world; + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + world->createPredictiveContactsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep); + } + }; + virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE; + + struct UpdaterIntegrateTransforms : public btIParallelForBody + { + btScalar timeStep; + btRigidBody** rigidBodies; + btDiscreteDynamicsWorldMt* world; + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + world->integrateTransformsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep); + } + }; + virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, - btBroadphaseInterface* pairCache, - btConstraintSolverPoolMt* constraintSolver, // Note this should be a solver-pool for multi-threading - btConstraintSolver* constraintSolverMt, // single multi-threaded solver for large islands (or NULL) - btCollisionConfiguration* collisionConfiguration - ); + btDiscreteDynamicsWorldMt(btDispatcher * dispatcher, + btBroadphaseInterface * pairCache, + btConstraintSolverPoolMt * solverPool, // Note this should be a solver-pool for multi-threading + btConstraintSolver * constraintSolverMt, // single multi-threaded solver for large islands (or NULL) + btCollisionConfiguration * collisionConfiguration); virtual ~btDiscreteDynamicsWorldMt(); - virtual int stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE; + virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE; }; -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h index 42d8fc0de3..eadd8c12e7 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -24,150 +24,150 @@ class btActionInterface; class btConstraintSolver; class btDynamicsWorld; - /// Type for the callback for each tick -typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); +typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep); enum btDynamicsWorldType { - BT_SIMPLE_DYNAMICS_WORLD=1, - BT_DISCRETE_DYNAMICS_WORLD=2, - BT_CONTINUOUS_DYNAMICS_WORLD=3, - BT_SOFT_RIGID_DYNAMICS_WORLD=4, - BT_GPU_DYNAMICS_WORLD=5, - BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6 + BT_SIMPLE_DYNAMICS_WORLD = 1, + BT_DISCRETE_DYNAMICS_WORLD = 2, + BT_CONTINUOUS_DYNAMICS_WORLD = 3, + BT_SOFT_RIGID_DYNAMICS_WORLD = 4, + BT_GPU_DYNAMICS_WORLD = 5, + BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. class btDynamicsWorld : public btCollisionWorld { - protected: - btInternalTickCallback m_internalTickCallback; - btInternalTickCallback m_internalPreTickCallback; - void* m_worldUserInfo; + btInternalTickCallback m_internalTickCallback; + btInternalTickCallback m_internalPreTickCallback; + void* m_worldUserInfo; - btContactSolverInfo m_solverInfo; + btContactSolverInfo m_solverInfo; public: - + btDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphase, btCollisionConfiguration* collisionConfiguration) + : btCollisionWorld(dispatcher, broadphase, collisionConfiguration), m_internalTickCallback(0), m_internalPreTickCallback(0), m_worldUserInfo(0) + { + } - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0) - { - } + virtual ~btDynamicsWorld() + { + } - virtual ~btDynamicsWorld() - { - } - - ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. - ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. - ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. - ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - - virtual void debugDrawWorld() = 0; - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) - { - (void)constraint; (void)disableCollisionsBetweenLinkedBodies; - } + ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. + ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. + ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. + ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0; - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} + virtual void debugDrawWorld() = 0; - virtual void addAction(btActionInterface* action) = 0; + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false) + { + (void)constraint; + (void)disableCollisionsBetweenLinkedBodies; + } - virtual void removeAction(btActionInterface* action) = 0; + virtual void removeConstraint(btTypedConstraint* constraint) { (void)constraint; } - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - virtual void setGravity(const btVector3& gravity) = 0; - virtual btVector3 getGravity () const = 0; + virtual void addAction(btActionInterface* action) = 0; - virtual void synchronizeMotionStates() = 0; + virtual void removeAction(btActionInterface* action) = 0; - virtual void addRigidBody(btRigidBody* body) = 0; + //once a rigidbody is added to the dynamics world, it will get this gravity assigned + //existing rigidbodies in the world get gravity assigned too, during this method + virtual void setGravity(const btVector3& gravity) = 0; + virtual btVector3 getGravity() const = 0; - virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; + virtual void synchronizeMotionStates() = 0; - virtual void removeRigidBody(btRigidBody* body) = 0; + virtual void addRigidBody(btRigidBody* body) = 0; - virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; - virtual btConstraintSolver* getConstraintSolver() = 0; - - virtual int getNumConstraints() const { return 0; } - - virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - - virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } + virtual void removeRigidBody(btRigidBody* body) = 0; - virtual btDynamicsWorldType getWorldType() const=0; + virtual void setConstraintSolver(btConstraintSolver* solver) = 0; - virtual void clearForces() = 0; + virtual btConstraintSolver* getConstraintSolver() = 0; - /// Set the callback for when an internal tick (simulation substep) happens, optional user info - void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false) - { - if (isPreTick) - { - m_internalPreTickCallback = cb; - } else - { - m_internalTickCallback = cb; - } - m_worldUserInfo = worldUserInfo; - } + virtual int getNumConstraints() const { return 0; } - void setWorldUserInfo(void* worldUserInfo) - { - m_worldUserInfo = worldUserInfo; - } + virtual btTypedConstraint* getConstraint(int index) + { + (void)index; + return 0; + } - void* getWorldUserInfo() const - { - return m_worldUserInfo; - } + virtual const btTypedConstraint* getConstraint(int index) const + { + (void)index; + return 0; + } + + virtual btDynamicsWorldType getWorldType() const = 0; + + virtual void clearForces() = 0; - btContactSolverInfo& getSolverInfo() + /// Set the callback for when an internal tick (simulation substep) happens, optional user info + void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo = 0, bool isPreTick = false) + { + if (isPreTick) { - return m_solverInfo; + m_internalPreTickCallback = cb; } - - const btContactSolverInfo& getSolverInfo() const + else { - return m_solverInfo; + m_internalTickCallback = cb; } - - - ///obsolete, use addAction instead. - virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use addAction instead. - virtual void addCharacter(btActionInterface* character) {(void)character;} - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character) {(void)character;} - - + m_worldUserInfo = worldUserInfo; + } + + void setWorldUserInfo(void* worldUserInfo) + { + m_worldUserInfo = worldUserInfo; + } + + void* getWorldUserInfo() const + { + return m_worldUserInfo; + } + + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + + const btContactSolverInfo& getSolverInfo() const + { + return m_solverInfo; + } + + ///obsolete, use addAction instead. + virtual void addVehicle(btActionInterface* vehicle) { (void)vehicle; } + ///obsolete, use removeAction instead + virtual void removeVehicle(btActionInterface* vehicle) { (void)vehicle; } + ///obsolete, use addAction instead. + virtual void addCharacter(btActionInterface* character) { (void)character; } + ///obsolete, use removeAction instead + virtual void removeCharacter(btActionInterface* character) { (void)character; } }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btDynamicsWorldDoubleData { - btContactSolverInfoDoubleData m_solverInfo; - btVector3DoubleData m_gravity; + btContactSolverInfoDoubleData m_solverInfo; + btVector3DoubleData m_gravity; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btDynamicsWorldFloatData { - btContactSolverInfoFloatData m_solverInfo; - btVector3FloatData m_gravity; + btContactSolverInfoFloatData m_solverInfo; + btVector3FloatData m_gravity; }; - -#endif //BT_DYNAMICS_WORLD_H - - +#endif //BT_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp index ca0714fcfa..f4bcabada2 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @@ -22,36 +22,34 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" //'temporarily' global variables -btScalar gDeactivationTime = btScalar(2.); -bool gDisableDeactivation = false; +btScalar gDeactivationTime = btScalar(2.); +bool gDisableDeactivation = false; static int uniqueId = 0; - btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { setupRigidBody(constructionInfo); } -btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) +btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia) { - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); + btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia); setupRigidBody(cinfo); } -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) +void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { - - m_internalType=CO_RIGID_BODY; + m_internalType = CO_RIGID_BODY; m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - m_angularFactor.setValue(1,1,1); - m_linearFactor.setValue(1,1,1); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + m_angularFactor.setValue(1, 1, 1); + m_linearFactor.setValue(1, 1, 1); m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; @@ -67,48 +65,44 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& if (m_optionalMotionState) { m_optionalMotionState->getWorldTransform(m_worldTransform); - } else + } + else { m_worldTransform = constructionInfo.m_startWorldTransform; } m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - + m_interpolationLinearVelocity.setValue(0, 0, 0); + m_interpolationAngularVelocity.setValue(0, 0, 0); + //moved to btCollisionObject m_friction = constructionInfo.m_friction; m_rollingFriction = constructionInfo.m_rollingFriction; - m_spinningFriction = constructionInfo.m_spinningFriction; - + m_spinningFriction = constructionInfo.m_spinningFriction; + m_restitution = constructionInfo.m_restitution; - setCollisionShape( constructionInfo.m_collisionShape ); + setCollisionShape(constructionInfo.m_collisionShape); m_debugBodyId = uniqueId++; - + setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); updateInertiaTensor(); m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; - m_deltaLinearVelocity.setZero(); m_deltaAngularVelocity.setZero(); - m_invMass = m_inverseMass*m_linearFactor; + m_invMass = m_inverseMass * m_linearFactor; m_pushVelocity.setZero(); m_turnVelocity.setZero(); - - - } - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) +void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); + btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } -void btRigidBody::saveKinematicState(btScalar timeStep) +void btRigidBody::saveKinematicState(btScalar timeStep) { //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities if (timeStep != btScalar(0.)) @@ -116,25 +110,22 @@ void btRigidBody::saveKinematicState(btScalar timeStep) //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform if (getMotionState()) getMotionState()->getWorldTransform(m_worldTransform); - btVector3 linVel,angVel; - - btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); + btVector3 linVel, angVel; + + btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity); m_interpolationLinearVelocity = m_linearVelocity; m_interpolationAngularVelocity = m_angularVelocity; m_interpolationWorldTransform = m_worldTransform; //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); } } - -void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const + +void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const { - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); + getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax); } - - - -void btRigidBody::setGravity(const btVector3& acceleration) +void btRigidBody::setGravity(const btVector3& acceleration) { if (m_inverseMass != btScalar(0.0)) { @@ -143,22 +134,14 @@ void btRigidBody::setGravity(const btVector3& acceleration) m_gravity_acceleration = acceleration; } - - - - - void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } - - - ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping -void btRigidBody::applyDamping(btScalar timeStep) +void btRigidBody::applyDamping(btScalar timeStep) { //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway @@ -168,8 +151,8 @@ void btRigidBody::applyDamping(btScalar timeStep) m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); #else - m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); - m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); + m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep); #endif if (m_additionalDamping) @@ -182,7 +165,6 @@ void btRigidBody::applyDamping(btScalar timeStep) m_angularVelocity *= m_additionalDampingFactor; m_linearVelocity *= m_additionalDampingFactor; } - btScalar speed = m_linearVelocity.length(); if (speed < m_linearDamping) @@ -191,10 +173,11 @@ void btRigidBody::applyDamping(btScalar timeStep) if (speed > dampVel) { btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else + m_linearVelocity -= dir * dampVel; + } + else { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } @@ -205,30 +188,28 @@ void btRigidBody::applyDamping(btScalar timeStep) if (angSpeed > angDampVel) { btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else + m_angularVelocity -= dir * angDampVel; + } + else { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } } } - void btRigidBody::applyGravity() { if (isStaticOrKinematicObject()) return; - - applyCentralForce(m_gravity); + applyCentralForce(m_gravity); } void btRigidBody::proceedToTransform(const btTransform& newTrans) { - setCenterOfMassTransform( newTrans ); + setCenterOfMassTransform(newTrans); } - void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { @@ -236,7 +217,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; m_inverseMass = btScalar(0.); - } else + } + else { m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); m_inverseMass = btScalar(1.0) / mass; @@ -244,50 +226,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) //Fg = m * a m_gravity = mass * m_gravity_acceleration; - - m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); - m_invMass = m_linearFactor*m_inverseMass; + m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + + m_invMass = m_linearFactor * m_inverseMass; } - -void btRigidBody::updateInertiaTensor() +void btRigidBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); } - - btVector3 btRigidBody::getLocalInertia() const { - btVector3 inertiaLocal; const btVector3 inertia = m_invInertiaLocal; inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); return inertiaLocal; } inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, - const btMatrix3x3 &I) + const btMatrix3x3& I) { - const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); + const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0); return w2; } inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt, - const btMatrix3x3 &I) + const btMatrix3x3& I) { - btMatrix3x3 w1x, Iw1x; - const btVector3 Iwi = (I*w1); + const btVector3 Iwi = (I * w1); w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]); Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]); - const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt; + const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt; return dfw1; } @@ -295,58 +272,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc { btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); + btVector3 tmp = inertiaTensorWorld * getAngularVelocity(); btVector3 gf = getAngularVelocity().cross(tmp); btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*maxGyroscopicForce) + if (l2 > maxGyroscopicForce * maxGyroscopicForce) { - gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; + gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce; } return gf; } - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const -{ +{ btVector3 idl = getLocalInertia(); btVector3 omega1 = getAngularVelocity(); btQuaternion q = getWorldTransform().getRotation(); - + // Convert to body coordinates btVector3 omegab = quatRotate(q.inverse(), omega1); btMatrix3x3 Ib; - Ib.setValue(idl.x(),0,0, - 0,idl.y(),0, - 0,0,idl.z()); - - btVector3 ibo = Ib*omegab; + Ib.setValue(idl.x(), 0, 0, + 0, idl.y(), 0, + 0, 0, idl.z()); + + btVector3 ibo = Ib * omegab; // Residual vector btVector3 f = step * omegab.cross(ibo); - + btMatrix3x3 skew0; omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); - btVector3 om = Ib*omegab; + btVector3 om = Ib * omegab; btMatrix3x3 skew1; - om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); - + om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]); + // Jacobian - btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; - -// btMatrix3x3 Jinv = J.inverse(); -// btVector3 omega_div = Jinv*f; + btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step; + + // btMatrix3x3 Jinv = J.inverse(); + // btVector3 omega_div = Jinv*f; btVector3 omega_div = J.solve33(f); - + // Single Newton-Raphson update - omegab = omegab - omega_div;//Solve33(J, f); + omegab = omegab - omega_div; //Solve33(J, f); // Back to world coordinates - btVector3 omega2 = quatRotate(q,omegab); - btVector3 gf = omega2-omega1; + btVector3 omega2 = quatRotate(q, omegab); + btVector3 gf = omega2 - omega1; return gf; } - - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const { // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. @@ -361,7 +335,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con m_worldTransform.getBasis().transpose(); // use newtons method to find implicit solution for new angular velocity (w') - // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 + // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 // df/dw' = I + 1xIw'*step + w'xI*step btVector3 w1 = w0; @@ -383,8 +357,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con return gf; } - -void btRigidBody::integrateVelocities(btScalar step) +void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; @@ -393,30 +366,28 @@ void btRigidBody::integrateVelocities(btScalar step) m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; #define MAX_ANGVEL SIMD_HALF_PI - /// clamp angular velocity. collision calculations will fail on higher angular velocities + /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) + if (angvel * step > MAX_ANGVEL) { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; + m_angularVelocity *= (MAX_ANGVEL / step) / angvel; } - } btQuaternion btRigidBody::getOrientation() const { - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; + btQuaternion orn; + m_worldTransform.getBasis().getRotation(orn); + return orn; } - - + void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - if (isKinematicObject()) { m_interpolationWorldTransform = m_worldTransform; - } else + } + else { m_interpolationWorldTransform = xform; } @@ -426,10 +397,6 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) updateInertiaTensor(); } - - - - void btRigidBody::addConstraintRef(btTypedConstraint* c) { ///disable collision with the 'other' body @@ -450,39 +417,39 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c) { colObjB->setIgnoreCollisionCheck(colObjA, true); } - } + } } void btRigidBody::removeConstraintRef(btTypedConstraint* c) { int index = m_constraintRefs.findLinearSearch(c); //don't remove constraints that are not referenced - if(index < m_constraintRefs.size()) - { - m_constraintRefs.remove(c); - btCollisionObject* colObjA = &c->getRigidBodyA(); - btCollisionObject* colObjB = &c->getRigidBodyB(); - if (colObjA == this) - { - colObjA->setIgnoreCollisionCheck(colObjB, false); - } - else - { - colObjB->setIgnoreCollisionCheck(colObjA, false); - } - } + if (index < m_constraintRefs.size()) + { + m_constraintRefs.remove(c); + btCollisionObject* colObjA = &c->getRigidBodyA(); + btCollisionObject* colObjB = &c->getRigidBodyB(); + if (colObjA == this) + { + colObjA->setIgnoreCollisionCheck(colObjB, false); + } + else + { + colObjB->setIgnoreCollisionCheck(colObjA, false); + } + } } -int btRigidBody::calculateSerializeBufferSize() const +int btRigidBody::calculateSerializeBufferSize() const { int sz = sizeof(btRigidBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const { - btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; + btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer; btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); @@ -504,7 +471,7 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; - rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; + rbd->m_linearSleepingThreshold = m_linearSleepingThreshold; rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; // Fill padding with zeros to appease msan. @@ -515,13 +482,9 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali return btRigidBodyDataName; } - - void btRigidBody::serializeSingleObject(class btSerializer* serializer) const { - btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); + btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this); } - - diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h index 372245031b..05f270a4b8 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h @@ -25,209 +25,195 @@ class btCollisionShape; class btMotionState; class btTypedConstraint; - extern btScalar gDeactivationTime; extern bool gDisableDeactivation; #ifdef BT_USE_DOUBLE_PRECISION -#define btRigidBodyData btRigidBodyDoubleData -#define btRigidBodyDataName "btRigidBodyDoubleData" +#define btRigidBodyData btRigidBodyDoubleData +#define btRigidBodyDataName "btRigidBodyDoubleData" #else -#define btRigidBodyData btRigidBodyFloatData -#define btRigidBodyDataName "btRigidBodyFloatData" -#endif //BT_USE_DOUBLE_PRECISION - +#define btRigidBodyData btRigidBodyFloatData +#define btRigidBodyDataName "btRigidBodyFloatData" +#endif //BT_USE_DOUBLE_PRECISION -enum btRigidBodyFlags +enum btRigidBodyFlags { BT_DISABLE_WORLD_GRAVITY = 1, ///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards. ///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY = 8, BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY, }; - ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. -///There are 3 types of rigid bodies: +///There are 3 types of rigid bodies: ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. ///- B) Fixed objects with zero mass. They are not moving (basically collision objects) -///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. +///- C) Kinematic objects, which are objects without mass, but the user can move them. There is one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. ///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time. ///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects) -class btRigidBody : public btCollisionObject +class btRigidBody : public btCollisionObject { - - btMatrix3x3 m_invInertiaTensorWorld; - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btScalar m_inverseMass; - btVector3 m_linearFactor; - - btVector3 m_gravity; - btVector3 m_gravity_acceleration; - btVector3 m_invInertiaLocal; - btVector3 m_totalForce; - btVector3 m_totalTorque; - - btScalar m_linearDamping; - btScalar m_angularDamping; - - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; - - - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; + btMatrix3x3 m_invInertiaTensorWorld; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btScalar m_inverseMass; + btVector3 m_linearFactor; + + btVector3 m_gravity; + btVector3 m_gravity_acceleration; + btVector3 m_invInertiaLocal; + btVector3 m_totalForce; + btVector3 m_totalTorque; + + btScalar m_linearDamping; + btScalar m_angularDamping; + + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; + + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; //m_optionalMotionState allows to automatic synchronize the world transform for active objects - btMotionState* m_optionalMotionState; + btMotionState* m_optionalMotionState; //keep track of typed constraints referencing this rigid body, to disable collision between linked bodies btAlignedObjectArray<btTypedConstraint*> m_constraintRefs; - int m_rigidbodyFlags; - - int m_debugBodyId; - + int m_rigidbodyFlags; -protected: - - ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity); - btVector3 m_deltaAngularVelocity; - btVector3 m_angularFactor; - btVector3 m_invMass; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; + int m_debugBodyId; +protected: + ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity); + btVector3 m_deltaAngularVelocity; + btVector3 m_angularFactor; + btVector3 m_invMass; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; public: - - ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) - ///You can use the motion state to synchronize the world transform between physics and graphics objects. + ///You can use the motion state to synchronize the world transform between physics and graphics objects. ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, ///m_startWorldTransform is only used when you don't provide a motion state. - struct btRigidBodyConstructionInfo + struct btRigidBodyConstructionInfo { - btScalar m_mass; + btScalar m_mass; ///When a motionState is provided, the rigid body will initialize its world transform from the motion state ///In this case, m_startWorldTransform is ignored. - btMotionState* m_motionState; - btTransform m_startWorldTransform; + btMotionState* m_motionState; + btTransform m_startWorldTransform; - btCollisionShape* m_collisionShape; - btVector3 m_localInertia; - btScalar m_linearDamping; - btScalar m_angularDamping; + btCollisionShape* m_collisionShape; + btVector3 m_localInertia; + btScalar m_linearDamping; + btScalar m_angularDamping; ///best simulation results when friction is non-zero - btScalar m_friction; + btScalar m_friction; ///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever. ///See Bullet/Demos/RollingFrictionDemo for usage - btScalar m_rollingFriction; - btScalar m_spinningFriction;//torsional friction around contact normal - + btScalar m_rollingFriction; + btScalar m_spinningFriction; //torsional friction around contact normal + ///best simulation results using zero restitution. - btScalar m_restitution; + btScalar m_restitution; - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; - - btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): - m_mass(mass), - m_motionState(motionState), - m_collisionShape(collisionShape), - m_localInertia(localInertia), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.)), - m_friction(btScalar(0.5)), - m_rollingFriction(btScalar(0)), - m_spinningFriction(btScalar(0)), - m_restitution(btScalar(0.)), - m_linearSleepingThreshold(btScalar(0.8)), - m_angularSleepingThreshold(btScalar(1.f)), - m_additionalDamping(false), - m_additionalDampingFactor(btScalar(0.005)), - m_additionalLinearDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingFactor(btScalar(0.01)) + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; + + btRigidBodyConstructionInfo(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)) : m_mass(mass), + m_motionState(motionState), + m_collisionShape(collisionShape), + m_localInertia(localInertia), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.)), + m_friction(btScalar(0.5)), + m_rollingFriction(btScalar(0)), + m_spinningFriction(btScalar(0)), + m_restitution(btScalar(0.)), + m_linearSleepingThreshold(btScalar(0.8)), + m_angularSleepingThreshold(btScalar(1.f)), + m_additionalDamping(false), + m_additionalDampingFactor(btScalar(0.005)), + m_additionalLinearDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingFactor(btScalar(0.01)) { m_startWorldTransform.setIdentity(); } }; ///btRigidBody constructor using construction info - btRigidBody( const btRigidBodyConstructionInfo& constructionInfo); + btRigidBody(const btRigidBodyConstructionInfo& constructionInfo); - ///btRigidBody constructor for backwards compatibility. + ///btRigidBody constructor for backwards compatibility. ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) - btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)); - + btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)); virtual ~btRigidBody() - { - //No constraints should point to this rigidbody - //Remove constraints from the dynamics world before you delete the related rigidbodies. - btAssert(m_constraintRefs.size()==0); - } + { + //No constraints should point to this rigidbody + //Remove constraints from the dynamics world before you delete the related rigidbodies. + btAssert(m_constraintRefs.size() == 0); + } protected: - ///setupRigidBody is only used internally by the constructor - void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); + void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); public: + void proceedToTransform(const btTransform& newTrans); - void proceedToTransform(const btTransform& newTrans); - ///to keep collision detection and dynamics separate we don't store a rigidbody pointer ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast - static const btRigidBody* upcast(const btCollisionObject* colObj) + static const btRigidBody* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) return (const btRigidBody*)colObj; return 0; } - static btRigidBody* upcast(btCollisionObject* colObj) + static btRigidBody* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) return (btRigidBody*)colObj; return 0; } - + /// continuous collision detection needs prediction - void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ; - - void saveKinematicState(btScalar step); - - void applyGravity(); - - void setGravity(const btVector3& acceleration); + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + + void saveKinematicState(btScalar step); + + void applyGravity(); + + void setGravity(const btVector3& acceleration); - const btVector3& getGravity() const + const btVector3& getGravity() const { return m_gravity_acceleration; } - void setDamping(btScalar lin_damping, btScalar ang_damping); + void setDamping(btScalar lin_damping, btScalar ang_damping); btScalar getLinearDamping() const { @@ -249,18 +235,20 @@ public: return m_angularSleepingThreshold; } - void applyDamping(btScalar timeStep); + void applyDamping(btScalar timeStep); - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + { return m_collisionShape; } - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { - return m_collisionShape; + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + { + return m_collisionShape; } - - void setMassProps(btScalar mass, const btVector3& inertia); - + + void setMassProps(btScalar mass, const btVector3& inertia); + const btVector3& getLinearFactor() const { return m_linearFactor; @@ -268,20 +256,21 @@ public: void setLinearFactor(const btVector3& linearFactor) { m_linearFactor = linearFactor; - m_invMass = m_linearFactor*m_inverseMass; + m_invMass = m_linearFactor * m_inverseMass; } - btScalar getInvMass() const { return m_inverseMass; } - const btMatrix3x3& getInvInertiaTensorWorld() const { - return m_invInertiaTensorWorld; + btScalar getInvMass() const { return m_inverseMass; } + const btMatrix3x3& getInvInertiaTensorWorld() const + { + return m_invInertiaTensorWorld; } - - void integrateVelocities(btScalar step); - void setCenterOfMassTransform(const btTransform& xform); + void integrateVelocities(btScalar step); + + void setCenterOfMassTransform(const btTransform& xform); - void applyCentralForce(const btVector3& force) + void applyCentralForce(const btVector3& force) { - m_totalForce += force*m_linearFactor; + m_totalForce += force * m_linearFactor; } const btVector3& getTotalForce() const @@ -293,90 +282,93 @@ public: { return m_totalTorque; }; - + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }; - void setInvInertiaDiagLocal(const btVector3& diagInvInertia) + void setInvInertiaDiagLocal(const btVector3& diagInvInertia) { m_invInertiaLocal = diagInvInertia; } - void setSleepingThresholds(btScalar linear,btScalar angular) + void setSleepingThresholds(btScalar linear, btScalar angular) { m_linearSleepingThreshold = linear; m_angularSleepingThreshold = angular; } - void applyTorque(const btVector3& torque) + void applyTorque(const btVector3& torque) { - m_totalTorque += torque*m_angularFactor; + m_totalTorque += torque * m_angularFactor; } - - void applyForce(const btVector3& force, const btVector3& rel_pos) + + void applyForce(const btVector3& force, const btVector3& rel_pos) { applyCentralForce(force); - applyTorque(rel_pos.cross(force*m_linearFactor)); + applyTorque(rel_pos.cross(force * m_linearFactor)); } - + void applyCentralImpulse(const btVector3& impulse) { - m_linearVelocity += impulse *m_linearFactor * m_inverseMass; + m_linearVelocity += impulse * m_linearFactor * m_inverseMass; } - - void applyTorqueImpulse(const btVector3& torque) + + void applyTorqueImpulse(const btVector3& torque) { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; } - - void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) + + void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) { if (m_inverseMass != btScalar(0.)) { applyCentralImpulse(impulse); if (m_angularFactor) { - applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor)); + applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor)); } } } - void clearForces() + void clearForces() { m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } - - void updateInertiaTensor(); - - const btVector3& getCenterOfMassPosition() const { - return m_worldTransform.getOrigin(); + + void updateInertiaTensor(); + + const btVector3& getCenterOfMassPosition() const + { + return m_worldTransform.getOrigin(); } btQuaternion getOrientation() const; - - const btTransform& getCenterOfMassTransform() const { - return m_worldTransform; + + const btTransform& getCenterOfMassTransform() const + { + return m_worldTransform; } - const btVector3& getLinearVelocity() const { - return m_linearVelocity; + const btVector3& getLinearVelocity() const + { + return m_linearVelocity; } - const btVector3& getAngularVelocity() const { - return m_angularVelocity; + const btVector3& getAngularVelocity() const + { + return m_angularVelocity; } - inline void setLinearVelocity(const btVector3& lin_vel) - { + { m_updateRevision++; - m_linearVelocity = lin_vel; + m_linearVelocity = lin_vel; } - inline void setAngularVelocity(const btVector3& ang_vel) - { + inline void setAngularVelocity(const btVector3& ang_vel) + { m_updateRevision++; - m_angularVelocity = ang_vel; + m_angularVelocity = ang_vel; } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -388,18 +380,13 @@ public: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } - void translate(const btVector3& v) + void translate(const btVector3& v) { - m_worldTransform.getOrigin() += v; + m_worldTransform.getOrigin() += v; } - - void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; - + void getAabb(btVector3& aabbMin, btVector3& aabbMax) const; - - - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); @@ -409,7 +396,6 @@ public: btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); return m_inverseMass + normal.dot(vec); - } SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const @@ -418,26 +404,25 @@ public: return axis.dot(vec); } - SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) + SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) { - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; - if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && - (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) + if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) && + (getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold)) { m_deactivationTime += timeStep; - } else + } + else { - m_deactivationTime=btScalar(0.); + m_deactivationTime = btScalar(0.); setActivationState(0); } - } - SIMD_FORCE_INLINE bool wantsSleeping() + SIMD_FORCE_INLINE bool wantsSleeping() { - if (getActivationState() == DISABLE_DEACTIVATION) return false; @@ -445,41 +430,39 @@ public: if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) return false; - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) return true; - if (m_deactivationTime> gDeactivationTime) + if (m_deactivationTime > gDeactivationTime) { return true; } return false; } - - - const btBroadphaseProxy* getBroadphaseProxy() const + const btBroadphaseProxy* getBroadphaseProxy() const { return m_broadphaseHandle; } - btBroadphaseProxy* getBroadphaseProxy() + btBroadphaseProxy* getBroadphaseProxy() { return m_broadphaseHandle; } - void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) + void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) { m_broadphaseHandle = broadphaseProxy; } //btMotionState allows to automatic synchronize the world transform for active objects - btMotionState* getMotionState() + btMotionState* getMotionState() { return m_optionalMotionState; } - const btMotionState* getMotionState() const + const btMotionState* getMotionState() const { return m_optionalMotionState; } - void setMotionState(btMotionState* motionState) + void setMotionState(btMotionState* motionState) { m_optionalMotionState = motionState; if (m_optionalMotionState) @@ -487,27 +470,27 @@ public: } //for experimental overriding of friction/contact solver func - int m_contactSolverType; - int m_frictionSolverType; + int m_contactSolverType; + int m_frictionSolverType; - void setAngularFactor(const btVector3& angFac) + void setAngularFactor(const btVector3& angFac) { m_updateRevision++; m_angularFactor = angFac; } - void setAngularFactor(btScalar angFac) + void setAngularFactor(btScalar angFac) { m_updateRevision++; - m_angularFactor.setValue(angFac,angFac,angFac); + m_angularFactor.setValue(angFac, angFac, angFac); } - const btVector3& getAngularFactor() const + const btVector3& getAngularFactor() const { return m_angularFactor; } //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - bool isInWorld() const + bool isInWorld() const { return (getBroadphaseProxy() != 0); } @@ -525,7 +508,7 @@ public: return m_constraintRefs.size(); } - void setFlags(int flags) + void setFlags(int flags) { m_rigidbodyFlags = flags; } @@ -535,12 +518,9 @@ public: return m_rigidbodyFlags; } - - - ///perform implicit force computation in world space btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const; - + ///perform implicit force computation in body space (inertial frame) btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const; @@ -550,70 +530,66 @@ public: /////////////////////////////////////////////// - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; virtual void serializeSingleObject(class btSerializer* serializer) const; - }; //@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btRigidBodyFloatData +struct btRigidBodyFloatData { - btCollisionObjectFloatData m_collisionObjectData; - btMatrix3x3FloatData m_invInertiaTensorWorld; - btVector3FloatData m_linearVelocity; - btVector3FloatData m_angularVelocity; - btVector3FloatData m_angularFactor; - btVector3FloatData m_linearFactor; - btVector3FloatData m_gravity; - btVector3FloatData m_gravity_acceleration; - btVector3FloatData m_invInertiaLocal; - btVector3FloatData m_totalForce; - btVector3FloatData m_totalTorque; - float m_inverseMass; - float m_linearDamping; - float m_angularDamping; - float m_additionalDampingFactor; - float m_additionalLinearDampingThresholdSqr; - float m_additionalAngularDampingThresholdSqr; - float m_additionalAngularDampingFactor; - float m_linearSleepingThreshold; - float m_angularSleepingThreshold; - int m_additionalDamping; + btCollisionObjectFloatData m_collisionObjectData; + btMatrix3x3FloatData m_invInertiaTensorWorld; + btVector3FloatData m_linearVelocity; + btVector3FloatData m_angularVelocity; + btVector3FloatData m_angularFactor; + btVector3FloatData m_linearFactor; + btVector3FloatData m_gravity; + btVector3FloatData m_gravity_acceleration; + btVector3FloatData m_invInertiaLocal; + btVector3FloatData m_totalForce; + btVector3FloatData m_totalTorque; + float m_inverseMass; + float m_linearDamping; + float m_angularDamping; + float m_additionalDampingFactor; + float m_additionalLinearDampingThresholdSqr; + float m_additionalAngularDampingThresholdSqr; + float m_additionalAngularDampingFactor; + float m_linearSleepingThreshold; + float m_angularSleepingThreshold; + int m_additionalDamping; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btRigidBodyDoubleData +struct btRigidBodyDoubleData { - btCollisionObjectDoubleData m_collisionObjectData; - btMatrix3x3DoubleData m_invInertiaTensorWorld; - btVector3DoubleData m_linearVelocity; - btVector3DoubleData m_angularVelocity; - btVector3DoubleData m_angularFactor; - btVector3DoubleData m_linearFactor; - btVector3DoubleData m_gravity; - btVector3DoubleData m_gravity_acceleration; - btVector3DoubleData m_invInertiaLocal; - btVector3DoubleData m_totalForce; - btVector3DoubleData m_totalTorque; - double m_inverseMass; - double m_linearDamping; - double m_angularDamping; - double m_additionalDampingFactor; - double m_additionalLinearDampingThresholdSqr; - double m_additionalAngularDampingThresholdSqr; - double m_additionalAngularDampingFactor; - double m_linearSleepingThreshold; - double m_angularSleepingThreshold; - int m_additionalDamping; - char m_padding[4]; + btCollisionObjectDoubleData m_collisionObjectData; + btMatrix3x3DoubleData m_invInertiaTensorWorld; + btVector3DoubleData m_linearVelocity; + btVector3DoubleData m_angularVelocity; + btVector3DoubleData m_angularFactor; + btVector3DoubleData m_linearFactor; + btVector3DoubleData m_gravity; + btVector3DoubleData m_gravity_acceleration; + btVector3DoubleData m_invInertiaLocal; + btVector3DoubleData m_totalForce; + btVector3DoubleData m_totalTorque; + double m_inverseMass; + double m_linearDamping; + double m_angularDamping; + double m_additionalDampingFactor; + double m_additionalLinearDampingThresholdSqr; + double m_additionalAngularDampingThresholdSqr; + double m_additionalAngularDampingFactor; + double m_linearSleepingThreshold; + double m_angularSleepingThreshold; + int m_additionalDamping; + char m_padding[4]; }; - - -#endif //BT_RIGIDBODY_H - +#endif //BT_RIGIDBODY_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 6f63b87c80..8103390fb1 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -21,47 +21,40 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - /* Make sure this dummy function never changes so that it can be used by probes that are checking whether the library is actually installed. */ -extern "C" +extern "C" { - void btBulletDynamicsProbe (); - void btBulletDynamicsProbe () {} + void btBulletDynamicsProbe(); + void btBulletDynamicsProbe() {} } - - - -btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_ownsConstraintSolver(false), -m_gravity(0,0,-10) +btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration), + m_constraintSolver(constraintSolver), + m_ownsConstraintSolver(false), + m_gravity(0, 0, -10) { - } - btSimpleDynamicsWorld::~btSimpleDynamicsWorld() { if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); + btAlignedFree(m_constraintSolver); } -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { (void)fixedTimeStep; (void)maxSubSteps; - ///apply gravity, predict motion predictUnconstraintMotion(timeStep); - btDispatcherInfo& dispatchInfo = getDispatchInfo(); + btDispatcherInfo& dispatchInfo = getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); @@ -74,17 +67,17 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b if (numManifolds) { btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); - + btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1); - m_constraintSolver->allSolved(infoGlobal,m_debugDrawer); + m_constraintSolver->prepareSolve(0, numManifolds); + m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1); + m_constraintSolver->allSolved(infoGlobal, m_debugDrawer); } ///integrate transforms integrateTransforms(timeStep); - + updateAabbs(); synchronizeMotionStates(); @@ -92,29 +85,27 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b clearForces(); return 1; - } -void btSimpleDynamicsWorld::clearForces() +void btSimpleDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; - + btRigidBody* body = btRigidBody::upcast(colObj); if (body) { body->clearForces(); } } -} - +} -void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) +void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -125,17 +116,17 @@ void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) } } -btVector3 btSimpleDynamicsWorld::getGravity () const +btVector3 btSimpleDynamicsWorld::getGravity() const { return m_gravity; } -void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) +void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) { btCollisionWorld::removeCollisionObject(body); } -void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btRigidBody* body = btRigidBody::upcast(collisionObject); if (body) @@ -144,8 +135,7 @@ void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionOb btCollisionWorld::removeCollisionObject(collisionObject); } - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) { body->setGravity(m_gravity); @@ -155,37 +145,32 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) } } -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { body->setGravity(m_gravity); if (body->getCollisionShape()) { - addCollisionObject(body,group,mask); + addCollisionObject(body, group, mask); } } - -void btSimpleDynamicsWorld::debugDrawWorld() +void btSimpleDynamicsWorld::debugDrawWorld() { - } - -void btSimpleDynamicsWorld::addAction(btActionInterface* action) -{ +void btSimpleDynamicsWorld::addAction(btActionInterface* action) +{ } -void btSimpleDynamicsWorld::removeAction(btActionInterface* action) +void btSimpleDynamicsWorld::removeAction(btActionInterface* action) { - } - -void btSimpleDynamicsWorld::updateAabbs() +void btSimpleDynamicsWorld::updateAabbs() { btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -193,19 +178,19 @@ void btSimpleDynamicsWorld::updateAabbs() { if (body->isActive() && (!body->isStaticObject())) { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 minAabb, maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1); } } } } -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) +void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) { btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -214,17 +199,15 @@ void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) if (body->isActive() && (!body->isStaticObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); + body->proceedToTransform(predictedTrans); } } } } - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -235,20 +218,19 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) if (body->isActive()) { body->applyGravity(); - body->integrateVelocities( timeStep); + body->integrateVelocities(timeStep); body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); } } } } } - -void btSimpleDynamicsWorld::synchronizeMotionStates() +void btSimpleDynamicsWorld::synchronizeMotionStates() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -260,11 +242,9 @@ void btSimpleDynamicsWorld::synchronizeMotionStates() } } } - } - -void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 44b7e7fb34..12be231c7f 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -27,63 +27,58 @@ class btConstraintSolver; class btSimpleDynamicsWorld : public btDynamicsWorld { protected: + btConstraintSolver* m_constraintSolver; - btConstraintSolver* m_constraintSolver; + bool m_ownsConstraintSolver; - bool m_ownsConstraintSolver; - - void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransforms(btScalar timeStep); - - btVector3 m_gravity; - -public: + void predictUnconstraintMotion(btScalar timeStep); + void integrateTransforms(btScalar timeStep); + btVector3 m_gravity; +public: ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver - btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); virtual ~btSimpleDynamicsWorld(); - + ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); - virtual void setGravity(const btVector3& gravity); + virtual void setGravity(const btVector3& gravity); - virtual btVector3 getGravity () const; + virtual btVector3 getGravity() const; - virtual void addRigidBody(btRigidBody* body); + virtual void addRigidBody(btRigidBody* body); - virtual void addRigidBody(btRigidBody* body, int group, int mask); + virtual void addRigidBody(btRigidBody* body, int group, int mask); - virtual void removeRigidBody(btRigidBody* body); + virtual void removeRigidBody(btRigidBody* body); - virtual void debugDrawWorld(); - - virtual void addAction(btActionInterface* action); + virtual void debugDrawWorld(); - virtual void removeAction(btActionInterface* action); + virtual void addAction(btActionInterface* action); + + virtual void removeAction(btActionInterface* action); ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void updateAabbs(); + virtual void removeCollisionObject(btCollisionObject* collisionObject); + + virtual void updateAabbs(); - virtual void synchronizeMotionStates(); + virtual void synchronizeMotionStates(); - virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver* solver); virtual btConstraintSolver* getConstraintSolver(); - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { return BT_SIMPLE_DYNAMICS_WORLD; } - virtual void clearForces(); - + virtual void clearForces(); }; -#endif //BT_SIMPLE_DYNAMICS_WORLD_H +#endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp index fc54f0ba6e..17287aa82a 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btScalar.h" #include "LinearMath/btThreads.h" #include "btSimulationIslandManagerMt.h" @@ -27,273 +26,259 @@ subject to the following restrictions: //#include <stdio.h> #include "LinearMath/btQuickprof.h" - -SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints ) +SIMD_FORCE_INLINE int calcBatchCost(int bodies, int manifolds, int constraints) { - // rough estimate of the cost of a batch, used for merging - int batchCost = bodies + 8 * manifolds + 4 * constraints; - return batchCost; + // rough estimate of the cost of a batch, used for merging + int batchCost = bodies + 8 * manifolds + 4 * constraints; + return batchCost; } - -SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island ) +SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island) { - return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() ); + return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size()); } - btSimulationIslandManagerMt::btSimulationIslandManagerMt() { - m_minimumSolverBatchSize = calcBatchCost(0, 128, 0); - m_batchIslandMinBodyCount = 32; - m_islandDispatch = parallelIslandDispatch; - m_batchIsland = NULL; + m_minimumSolverBatchSize = calcBatchCost(0, 128, 0); + m_batchIslandMinBodyCount = 32; + m_islandDispatch = parallelIslandDispatch; + m_batchIsland = NULL; } - btSimulationIslandManagerMt::~btSimulationIslandManagerMt() { - for ( int i = 0; i < m_allocatedIslands.size(); ++i ) - { - delete m_allocatedIslands[ i ]; - } - m_allocatedIslands.resize( 0 ); - m_activeIslands.resize( 0 ); - m_freeIslands.resize( 0 ); + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + delete m_allocatedIslands[i]; + } + m_allocatedIslands.resize(0); + m_activeIslands.resize(0); + m_freeIslands.resize(0); } - -inline int getIslandId(const btPersistentManifold* lhs) +inline int getIslandId(const btPersistentManifold* lhs) { const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); - int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); + int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); return islandId; } - -SIMD_FORCE_INLINE int btGetConstraintIslandId( const btTypedConstraint* lhs ) +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); - return islandId; + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; } /// function object that routes calls to operator< class IslandBatchSizeSortPredicate { public: - bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const - { - int lCost = calcBatchCost( lhs ); - int rCost = calcBatchCost( rhs ); - return lCost > rCost; - } + bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const + { + int lCost = calcBatchCost(lhs); + int rCost = calcBatchCost(rhs); + return lCost > rCost; + } }; - class IslandBodyCapacitySortPredicate { public: - bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const - { - return lhs->bodyArray.capacity() > rhs->bodyArray.capacity(); - } + bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const + { + return lhs->bodyArray.capacity() > rhs->bodyArray.capacity(); + } }; - -void btSimulationIslandManagerMt::Island::append( const Island& other ) +void btSimulationIslandManagerMt::Island::append(const Island& other) { - // append bodies - for ( int i = 0; i < other.bodyArray.size(); ++i ) - { - bodyArray.push_back( other.bodyArray[ i ] ); - } - // append manifolds - for ( int i = 0; i < other.manifoldArray.size(); ++i ) - { - manifoldArray.push_back( other.manifoldArray[ i ] ); - } - // append constraints - for ( int i = 0; i < other.constraintArray.size(); ++i ) - { - constraintArray.push_back( other.constraintArray[ i ] ); - } + // append bodies + for (int i = 0; i < other.bodyArray.size(); ++i) + { + bodyArray.push_back(other.bodyArray[i]); + } + // append manifolds + for (int i = 0; i < other.manifoldArray.size(); ++i) + { + manifoldArray.push_back(other.manifoldArray[i]); + } + // append constraints + for (int i = 0; i < other.constraintArray.size(); ++i) + { + constraintArray.push_back(other.constraintArray[i]); + } } - -bool btIsBodyInIsland( const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj ) +bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj) { - for ( int i = 0; i < island.bodyArray.size(); ++i ) - { - if ( island.bodyArray[ i ] == obj ) - { - return true; - } - } - return false; + for (int i = 0; i < island.bodyArray.size(); ++i) + { + if (island.bodyArray[i] == obj) + { + return true; + } + } + return false; } - void btSimulationIslandManagerMt::initIslandPools() { - // reset island pools - int numElem = getUnionFind().getNumElements(); - m_lookupIslandFromId.resize( numElem ); - for ( int i = 0; i < m_lookupIslandFromId.size(); ++i ) - { - m_lookupIslandFromId[ i ] = NULL; - } - m_activeIslands.resize( 0 ); - m_freeIslands.resize( 0 ); - // check whether allocated islands are sorted by body capacity (largest to smallest) - int lastCapacity = 0; - bool isSorted = true; - for ( int i = 0; i < m_allocatedIslands.size(); ++i ) - { - Island* island = m_allocatedIslands[ i ]; - int cap = island->bodyArray.capacity(); - if ( cap > lastCapacity ) - { - isSorted = false; - break; - } - lastCapacity = cap; - } - if ( !isSorted ) - { - m_allocatedIslands.quickSort( IslandBodyCapacitySortPredicate() ); - } - - m_batchIsland = NULL; - // mark all islands free (but avoid deallocation) - for ( int i = 0; i < m_allocatedIslands.size(); ++i ) - { - Island* island = m_allocatedIslands[ i ]; - island->bodyArray.resize( 0 ); - island->manifoldArray.resize( 0 ); - island->constraintArray.resize( 0 ); - island->id = -1; - island->isSleeping = true; - m_freeIslands.push_back( island ); - } -} + // reset island pools + int numElem = getUnionFind().getNumElements(); + m_lookupIslandFromId.resize(numElem); + for (int i = 0; i < m_lookupIslandFromId.size(); ++i) + { + m_lookupIslandFromId[i] = NULL; + } + m_activeIslands.resize(0); + m_freeIslands.resize(0); + // check whether allocated islands are sorted by body capacity (largest to smallest) + int lastCapacity = 0; + bool isSorted = true; + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + Island* island = m_allocatedIslands[i]; + int cap = island->bodyArray.capacity(); + if (cap > lastCapacity) + { + isSorted = false; + break; + } + lastCapacity = cap; + } + if (!isSorted) + { + m_allocatedIslands.quickSort(IslandBodyCapacitySortPredicate()); + } + m_batchIsland = NULL; + // mark all islands free (but avoid deallocation) + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + Island* island = m_allocatedIslands[i]; + island->bodyArray.resize(0); + island->manifoldArray.resize(0); + island->constraintArray.resize(0); + island->id = -1; + island->isSleeping = true; + m_freeIslands.push_back(island); + } +} -btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland( int id ) +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id) { - Island* island = m_lookupIslandFromId[ id ]; - if ( island == NULL ) - { - // search for existing island - for ( int i = 0; i < m_activeIslands.size(); ++i ) - { - if ( m_activeIslands[ i ]->id == id ) - { - island = m_activeIslands[ i ]; - break; - } - } - m_lookupIslandFromId[ id ] = island; - } - return island; + Island* island = m_lookupIslandFromId[id]; + if (island == NULL) + { + // search for existing island + for (int i = 0; i < m_activeIslands.size(); ++i) + { + if (m_activeIslands[i]->id == id) + { + island = m_activeIslands[i]; + break; + } + } + m_lookupIslandFromId[id] = island; + } + return island; } - -btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland( int id, int numBodies ) +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland(int id, int numBodies) { - Island* island = NULL; - int allocSize = numBodies; - if ( numBodies < m_batchIslandMinBodyCount ) - { - if ( m_batchIsland ) - { - island = m_batchIsland; - m_lookupIslandFromId[ id ] = island; - // if we've made a large enough batch, - if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount ) - { - // next time start a new batch - m_batchIsland = NULL; - } - return island; - } - else - { - // need to allocate a batch island - allocSize = m_batchIslandMinBodyCount * 2; - } - } - btAlignedObjectArray<Island*>& freeIslands = m_freeIslands; - - // search for free island - if ( freeIslands.size() > 0 ) - { - // try to reuse a previously allocated island - int iFound = freeIslands.size(); - // linear search for smallest island that can hold our bodies - for ( int i = freeIslands.size() - 1; i >= 0; --i ) - { - if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize ) - { - iFound = i; - island = freeIslands[ i ]; - island->id = id; - break; - } - } - // if found, shrink array while maintaining ordering - if ( island ) - { - int iDest = iFound; - int iSrc = iDest + 1; - while ( iSrc < freeIslands.size() ) - { - freeIslands[ iDest++ ] = freeIslands[ iSrc++ ]; - } - freeIslands.pop_back(); - } - } - if ( island == NULL ) - { - // no free island found, allocate - island = new Island(); // TODO: change this to use the pool allocator - island->id = id; - island->bodyArray.reserve( allocSize ); - m_allocatedIslands.push_back( island ); - } - m_lookupIslandFromId[ id ] = island; - if ( numBodies < m_batchIslandMinBodyCount ) - { - m_batchIsland = island; - } - m_activeIslands.push_back( island ); - return island; -} + Island* island = NULL; + int allocSize = numBodies; + if (numBodies < m_batchIslandMinBodyCount) + { + if (m_batchIsland) + { + island = m_batchIsland; + m_lookupIslandFromId[id] = island; + // if we've made a large enough batch, + if (island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount) + { + // next time start a new batch + m_batchIsland = NULL; + } + return island; + } + else + { + // need to allocate a batch island + allocSize = m_batchIslandMinBodyCount * 2; + } + } + btAlignedObjectArray<Island*>& freeIslands = m_freeIslands; + // search for free island + if (freeIslands.size() > 0) + { + // try to reuse a previously allocated island + int iFound = freeIslands.size(); + // linear search for smallest island that can hold our bodies + for (int i = freeIslands.size() - 1; i >= 0; --i) + { + if (freeIslands[i]->bodyArray.capacity() >= allocSize) + { + iFound = i; + island = freeIslands[i]; + island->id = id; + break; + } + } + // if found, shrink array while maintaining ordering + if (island) + { + int iDest = iFound; + int iSrc = iDest + 1; + while (iSrc < freeIslands.size()) + { + freeIslands[iDest++] = freeIslands[iSrc++]; + } + freeIslands.pop_back(); + } + } + if (island == NULL) + { + // no free island found, allocate + island = new Island(); // TODO: change this to use the pool allocator + island->id = id; + island->bodyArray.reserve(allocSize); + m_allocatedIslands.push_back(island); + } + m_lookupIslandFromId[id] = island; + if (numBodies < m_batchIslandMinBodyCount) + { + m_batchIsland = island; + } + m_activeIslands.push_back(island); + return island; +} -void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld ) +void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) { - BT_PROFILE("buildIslands"); - + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore - + getUnionFind().sortIslands(); int numElem = getUnionFind().getNumElements(); - int endIslandIndex=1; + int endIslandIndex = 1; int startIslandIndex; //update the sleeping state for bodies, if all are sleeping - for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; - for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) { } @@ -302,21 +287,21 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl bool allSleeping = true; int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG || - colObj0->getActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState() == ACTIVE_TAG || + colObj0->getActivationState() == DISABLE_DEACTIVATION) { allSleeping = false; break; @@ -327,43 +312,43 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl if (allSleeping) { int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - colObj0->setActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState(ISLAND_SLEEPING); } } - } else + } + else { - int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) + if (colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState(WANTS_DEACTIVATION); colObj0->setDeactivationTime(0.f); } } @@ -372,352 +357,338 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl } } - -void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld ) +void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld) { - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - int endIslandIndex = 1; - int startIslandIndex; - int numElem = getUnionFind().getNumElements(); - - // create explicit islands and add bodies to each - for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex ) - { - int islandId = getUnionFind().getElement( startIslandIndex ).m_id; - - // find end index - for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ ) - { - } - // check if island is sleeping - bool islandSleeping = true; - for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ ) - { - int i = getUnionFind().getElement( iElem ).m_sz; - btCollisionObject* colObj = collisionObjects[ i ]; - if ( colObj->isActive() ) - { - islandSleeping = false; - } - } - if ( !islandSleeping ) - { - // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures - int numBodies = endIslandIndex - startIslandIndex; - Island* island = allocateIsland( islandId, numBodies ); - island->isSleeping = false; - - // add bodies to island - for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ ) - { - int i = getUnionFind().getElement( iElem ).m_sz; - btCollisionObject* colObj = collisionObjects[ i ]; - island->bodyArray.push_back( colObj ); - } - } - } + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + int endIslandIndex = 1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); -} + // create explicit islands and add bodies to each + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + // find end index + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + } + // check if island is sleeping + bool islandSleeping = true; + for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++) + { + int i = getUnionFind().getElement(iElem).m_sz; + btCollisionObject* colObj = collisionObjects[i]; + if (colObj->isActive()) + { + islandSleeping = false; + } + } + if (!islandSleeping) + { + // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures + int numBodies = endIslandIndex - startIslandIndex; + Island* island = allocateIsland(islandId, numBodies); + island->isSleeping = false; -void btSimulationIslandManagerMt::addManifoldsToIslands( btDispatcher* dispatcher ) -{ - // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island - int maxNumManifolds = dispatcher->getNumManifolds(); - for ( int i = 0; i < maxNumManifolds; i++ ) - { - btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal( i ); - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() ); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() ); - - ///@todo: check sleeping conditions! - if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) || - ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) ) - { - - //kinematic objects don't merge islands, but wake up all connected objects - if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING ) - { - if ( colObj0->hasContactResponse() ) - colObj1->activate(); - } - if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING ) - { - if ( colObj1->hasContactResponse() ) - colObj0->activate(); - } - //filtering for response - if ( dispatcher->needsResponse( colObj0, colObj1 ) ) - { - // scatter manifolds into various islands - int islandId = getIslandId( manifold ); - // if island not sleeping, - if ( Island* island = getIsland( islandId ) ) - { - island->manifoldArray.push_back( manifold ); - } - } - } - } + // add bodies to island + for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++) + { + int i = getUnionFind().getElement(iElem).m_sz; + btCollisionObject* colObj = collisionObjects[i]; + island->bodyArray.push_back(colObj); + } + } + } } - -void btSimulationIslandManagerMt::addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints ) +void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher) { - // walk constraints - for ( int i = 0; i < constraints.size(); i++ ) - { - // scatter constraints into various islands - btTypedConstraint* constraint = constraints[ i ]; - if ( constraint->isEnabled() ) - { - int islandId = btGetConstraintIslandId( constraint ); - // if island is not sleeping, - if ( Island* island = getIsland( islandId ) ) - { - island->constraintArray.push_back( constraint ); - } - } - } + // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island + int maxNumManifolds = dispatcher->getNumManifolds(); + for (int i = 0; i < maxNumManifolds; i++) + { + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) + { + //kinematic objects don't merge islands, but wake up all connected objects + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + //filtering for response + if (dispatcher->needsResponse(colObj0, colObj1)) + { + // scatter manifolds into various islands + int islandId = getIslandId(manifold); + // if island not sleeping, + if (Island* island = getIsland(islandId)) + { + island->manifoldArray.push_back(manifold); + } + } + } + } } +void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints) +{ + // walk constraints + for (int i = 0; i < constraints.size(); i++) + { + // scatter constraints into various islands + btTypedConstraint* constraint = constraints[i]; + if (constraint->isEnabled()) + { + int islandId = btGetConstraintIslandId(constraint); + // if island is not sleeping, + if (Island* island = getIsland(islandId)) + { + island->constraintArray.push_back(constraint); + } + } + } +} void btSimulationIslandManagerMt::mergeIslands() { - // sort islands in order of decreasing batch size - m_activeIslands.quickSort( IslandBatchSizeSortPredicate() ); - - // merge small islands to satisfy minimum batch size - // find first small batch island - int destIslandIndex = m_activeIslands.size(); - for ( int i = 0; i < m_activeIslands.size(); ++i ) - { - Island* island = m_activeIslands[ i ]; - int batchSize = calcBatchCost( island ); - if ( batchSize < m_minimumSolverBatchSize ) - { - destIslandIndex = i; - break; - } - } - int lastIndex = m_activeIslands.size() - 1; - while ( destIslandIndex < lastIndex ) - { - // merge islands from the back of the list - Island* island = m_activeIslands[ destIslandIndex ]; - int numBodies = island->bodyArray.size(); - int numManifolds = island->manifoldArray.size(); - int numConstraints = island->constraintArray.size(); - int firstIndex = lastIndex; - // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have - while ( true ) - { - Island* src = m_activeIslands[ firstIndex ]; - numBodies += src->bodyArray.size(); - numManifolds += src->manifoldArray.size(); - numConstraints += src->constraintArray.size(); - int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints ); - if ( batchCost >= m_minimumSolverBatchSize ) - { - break; - } - if ( firstIndex - 1 == destIslandIndex ) - { - break; - } - firstIndex--; - } - // reserve space for these pointers to minimize reallocation - island->bodyArray.reserve( numBodies ); - island->manifoldArray.reserve( numManifolds ); - island->constraintArray.reserve( numConstraints ); - // merge islands - for ( int i = firstIndex; i <= lastIndex; ++i ) - { - island->append( *m_activeIslands[ i ] ); - } - // shrink array to exclude the islands that were merged from - m_activeIslands.resize( firstIndex ); - lastIndex = firstIndex - 1; - destIslandIndex++; - } -} + // sort islands in order of decreasing batch size + m_activeIslands.quickSort(IslandBatchSizeSortPredicate()); + // merge small islands to satisfy minimum batch size + // find first small batch island + int destIslandIndex = m_activeIslands.size(); + for (int i = 0; i < m_activeIslands.size(); ++i) + { + Island* island = m_activeIslands[i]; + int batchSize = calcBatchCost(island); + if (batchSize < m_minimumSolverBatchSize) + { + destIslandIndex = i; + break; + } + } + int lastIndex = m_activeIslands.size() - 1; + while (destIslandIndex < lastIndex) + { + // merge islands from the back of the list + Island* island = m_activeIslands[destIslandIndex]; + int numBodies = island->bodyArray.size(); + int numManifolds = island->manifoldArray.size(); + int numConstraints = island->constraintArray.size(); + int firstIndex = lastIndex; + // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have + while (true) + { + Island* src = m_activeIslands[firstIndex]; + numBodies += src->bodyArray.size(); + numManifolds += src->manifoldArray.size(); + numConstraints += src->constraintArray.size(); + int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints); + if (batchCost >= m_minimumSolverBatchSize) + { + break; + } + if (firstIndex - 1 == destIslandIndex) + { + break; + } + firstIndex--; + } + // reserve space for these pointers to minimize reallocation + island->bodyArray.reserve(numBodies); + island->manifoldArray.reserve(numManifolds); + island->constraintArray.reserve(numConstraints); + // merge islands + for (int i = firstIndex; i <= lastIndex; ++i) + { + island->append(*m_activeIslands[i]); + } + // shrink array to exclude the islands that were merged from + m_activeIslands.resize(firstIndex); + lastIndex = firstIndex - 1; + destIslandIndex++; + } +} void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams) { - btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL; - btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL; - solver->solveGroup( &island.bodyArray[ 0 ], - island.bodyArray.size(), - manifolds, - island.manifoldArray.size(), - constraintsPtr, - island.constraintArray.size(), - *solverParams.m_solverInfo, - solverParams.m_debugDrawer, - solverParams.m_dispatcher - ); + btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL; + btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL; + solver->solveGroup(&island.bodyArray[0], + island.bodyArray.size(), + manifolds, + island.manifoldArray.size(), + constraintsPtr, + island.constraintArray.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher); } - -void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams ) +void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) { - BT_PROFILE( "serialIslandDispatch" ); - // serial dispatch - btAlignedObjectArray<Island*>& islands = *islandsPtr; - btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; - for ( int i = 0; i < islands.size(); ++i ) - { - solveIsland(solver, *islands[ i ], solverParams); - } + BT_PROFILE("serialIslandDispatch"); + // serial dispatch + btAlignedObjectArray<Island*>& islands = *islandsPtr; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + for (int i = 0; i < islands.size(); ++i) + { + solveIsland(solver, *islands[i], solverParams); + } } - struct UpdateIslandDispatcher : public btIParallelForBody { - btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr; - const btSimulationIslandManagerMt::SolverParams& m_solverParams; - - UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) - : m_islandsPtr(islandsPtr), m_solverParams(solverParams) - {} - - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - btConstraintSolver* solver = m_solverParams.m_solverPool; - for ( int i = iBegin; i < iEnd; ++i ) - { - btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ]; - btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams ); - } - } -}; + btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr; + const btSimulationIslandManagerMt::SolverParams& m_solverParams; + UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) + : m_islandsPtr(islandsPtr), m_solverParams(solverParams) + { + } + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + btConstraintSolver* solver = m_solverParams.m_solverPool; + for (int i = iBegin; i < iEnd; ++i) + { + btSimulationIslandManagerMt::Island* island = m_islandsPtr[i]; + btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams); + } + } +}; -void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams ) +void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) { - BT_PROFILE( "parallelIslandDispatch" ); - // - // if there are islands with many contacts, it may be faster to submit these - // large islands *serially* to a single parallel constraint solver, and then later - // submit the remaining smaller islands in parallel to multiple sequential solvers. - // - // Some task schedulers do not deal well with nested parallelFor loops. One implementation - // of OpenMP was actually slower than doing everything single-threaded. Intel TBB - // on the other hand, seems to do a pretty respectable job with it. - // - // When solving islands in parallel, the worst case performance happens when there - // is one very large island and then perhaps a smattering of very small - // islands -- one worker thread takes the large island and the remaining workers - // tear through the smaller islands and then sit idle waiting for the first worker - // to finish. Solving islands in parallel works best when there are numerous small - // islands, roughly equal in size. - // - // By contrast, the other approach -- the parallel constraint solver -- is only - // able to deliver a worthwhile speedup when the island is large. For smaller islands, - // it is difficult to extract a useful amount of parallelism -- the overhead of grouping - // the constraints into batches and sending the batches to worker threads can nullify - // any gains from parallelism. - // - - UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); - // We take advantage of the fact the islands are sorted in order of decreasing size - int iBegin = 0; - if (solverParams.m_solverMt) - { - while ( iBegin < islandsPtr->size() ) - { - btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ]; - if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching ) - { - // OK to submit the rest of the array in parallel - break; - } - // serial dispatch to parallel solver for large islands (if any) - solveIsland(solverParams.m_solverMt, *island, solverParams); - ++iBegin; - } - } - // parallel dispatch to sequential solvers for rest - btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher ); + BT_PROFILE("parallelIslandDispatch"); + // + // if there are islands with many contacts, it may be faster to submit these + // large islands *serially* to a single parallel constraint solver, and then later + // submit the remaining smaller islands in parallel to multiple sequential solvers. + // + // Some task schedulers do not deal well with nested parallelFor loops. One implementation + // of OpenMP was actually slower than doing everything single-threaded. Intel TBB + // on the other hand, seems to do a pretty respectable job with it. + // + // When solving islands in parallel, the worst case performance happens when there + // is one very large island and then perhaps a smattering of very small + // islands -- one worker thread takes the large island and the remaining workers + // tear through the smaller islands and then sit idle waiting for the first worker + // to finish. Solving islands in parallel works best when there are numerous small + // islands, roughly equal in size. + // + // By contrast, the other approach -- the parallel constraint solver -- is only + // able to deliver a worthwhile speedup when the island is large. For smaller islands, + // it is difficult to extract a useful amount of parallelism -- the overhead of grouping + // the constraints into batches and sending the batches to worker threads can nullify + // any gains from parallelism. + // + + UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); + // We take advantage of the fact the islands are sorted in order of decreasing size + int iBegin = 0; + if (solverParams.m_solverMt) + { + while (iBegin < islandsPtr->size()) + { + btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin]; + if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching) + { + // OK to submit the rest of the array in parallel + break; + } + // serial dispatch to parallel solver for large islands (if any) + solveIsland(solverParams.m_solverMt, *island, solverParams); + ++iBegin; + } + } + // parallel dispatch to sequential solvers for rest + btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher); } - ///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher, - btCollisionWorld* collisionWorld, - btAlignedObjectArray<btTypedConstraint*>& constraints, - const SolverParams& solverParams - ) +void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray<btTypedConstraint*>& constraints, + const SolverParams& solverParams) { BT_PROFILE("buildAndProcessIslands"); btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - buildIslands(dispatcher,collisionWorld); + buildIslands(dispatcher, collisionWorld); - if(!getSplitIslands()) + if (!getSplitIslands()) { - btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); - int maxNumManifolds = dispatcher->getNumManifolds(); - - for ( int i = 0; i < maxNumManifolds; i++ ) - { - btPersistentManifold* manifold = manifolds[ i ]; - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() ); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() ); - - ///@todo: check sleeping conditions! - if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) || - ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) ) - { - - //kinematic objects don't merge islands, but wake up all connected objects - if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING ) - { - if ( colObj0->hasContactResponse() ) - colObj1->activate(); - } - if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING ) - { - if ( colObj1->hasContactResponse() ) - colObj0->activate(); - } - } - } - btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL; - btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; - solver->solveGroup(&collisionObjects[0], - collisionObjects.size(), - manifolds, - maxNumManifolds, - constraintsPtr, - constraints.size(), - *solverParams.m_solverInfo, - solverParams.m_debugDrawer, - solverParams.m_dispatcher - ); + btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + + for (int i = 0; i < maxNumManifolds; i++) + { + btPersistentManifold* manifold = manifolds[i]; + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) + { + //kinematic objects don't merge islands, but wake up all connected objects + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + } + } + btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + solver->solveGroup(&collisionObjects[0], + collisionObjects.size(), + manifolds, + maxNumManifolds, + constraintsPtr, + constraints.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher); } else { - initIslandPools(); - - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - addBodiesToIslands( collisionWorld ); - addManifoldsToIslands( dispatcher ); - addConstraintsToIslands( constraints ); - - // m_activeIslands array should now contain all non-sleeping Islands, and each Island should - // have all the necessary bodies, manifolds and constraints. - - // if we want to merge islands with small batch counts, - if ( m_minimumSolverBatchSize > 1 ) - { - mergeIslands(); - } - // dispatch islands to solver - m_islandDispatch( &m_activeIslands, solverParams ); + initIslandPools(); + + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + addBodiesToIslands(collisionWorld); + addManifoldsToIslands(dispatcher); + addConstraintsToIslands(constraints); + + // m_activeIslands array should now contain all non-sleeping Islands, and each Island should + // have all the necessary bodies, manifolds and constraints. + + // if we want to merge islands with small batch counts, + if (m_minimumSolverBatchSize > 1) + { + mergeIslands(); + } + // dispatch islands to solver + m_islandDispatch(&m_activeIslands, solverParams); } } diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h index 563577a6f4..ab73a899f1 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h @@ -35,80 +35,78 @@ class btIDebugDraw; class btSimulationIslandManagerMt : public btSimulationIslandManager { public: - struct Island - { - // a simulation island consisting of bodies, manifolds and constraints, - // to be passed into a constraint solver. - btAlignedObjectArray<btCollisionObject*> bodyArray; - btAlignedObjectArray<btPersistentManifold*> manifoldArray; - btAlignedObjectArray<btTypedConstraint*> constraintArray; - int id; // island id - bool isSleeping; + struct Island + { + // a simulation island consisting of bodies, manifolds and constraints, + // to be passed into a constraint solver. + btAlignedObjectArray<btCollisionObject*> bodyArray; + btAlignedObjectArray<btPersistentManifold*> manifoldArray; + btAlignedObjectArray<btTypedConstraint*> constraintArray; + int id; // island id + bool isSleeping; - void append( const Island& other ); // add bodies, manifolds, constraints to my own - }; - struct SolverParams - { - btConstraintSolver* m_solverPool; - btConstraintSolver* m_solverMt; - btContactSolverInfo* m_solverInfo; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - }; - static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams); + void append(const Island& other); // add bodies, manifolds, constraints to my own + }; + struct SolverParams + { + btConstraintSolver* m_solverPool; + btConstraintSolver* m_solverMt; + btContactSolverInfo* m_solverInfo; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + }; + static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams); + + typedef void (*IslandDispatchFunc)(btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams); + static void serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams); + static void parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams); - typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams ); - static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams ); - static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams ); protected: - btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands - btAlignedObjectArray<Island*> m_activeIslands; // islands actively in use - btAlignedObjectArray<Island*> m_freeIslands; // islands ready to be reused - btAlignedObjectArray<Island*> m_lookupIslandFromId; // big lookup table to map islandId to Island pointer - Island* m_batchIsland; - int m_minimumSolverBatchSize; - int m_batchIslandMinBodyCount; - IslandDispatchFunc m_islandDispatch; + btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands + btAlignedObjectArray<Island*> m_activeIslands; // islands actively in use + btAlignedObjectArray<Island*> m_freeIslands; // islands ready to be reused + btAlignedObjectArray<Island*> m_lookupIslandFromId; // big lookup table to map islandId to Island pointer + Island* m_batchIsland; + int m_minimumSolverBatchSize; + int m_batchIslandMinBodyCount; + IslandDispatchFunc m_islandDispatch; + + Island* getIsland(int id); + virtual Island* allocateIsland(int id, int numBodies); + virtual void initIslandPools(); + virtual void addBodiesToIslands(btCollisionWorld* collisionWorld); + virtual void addManifoldsToIslands(btDispatcher* dispatcher); + virtual void addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints); + virtual void mergeIslands(); - Island* getIsland( int id ); - virtual Island* allocateIsland( int id, int numBodies ); - virtual void initIslandPools(); - virtual void addBodiesToIslands( btCollisionWorld* collisionWorld ); - virtual void addManifoldsToIslands( btDispatcher* dispatcher ); - virtual void addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints ); - virtual void mergeIslands(); - public: btSimulationIslandManagerMt(); virtual ~btSimulationIslandManagerMt(); - virtual void buildAndProcessIslands( btDispatcher* dispatcher, - btCollisionWorld* collisionWorld, - btAlignedObjectArray<btTypedConstraint*>& constraints, - const SolverParams& solverParams - ); + virtual void buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray<btTypedConstraint*>& constraints, + const SolverParams& solverParams); - virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + virtual void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); - int getMinimumSolverBatchSize() const - { - return m_minimumSolverBatchSize; - } - void setMinimumSolverBatchSize( int sz ) - { - m_minimumSolverBatchSize = sz; - } - IslandDispatchFunc getIslandDispatchFunction() const - { - return m_islandDispatch; - } - // allow users to set their own dispatch function for multithreaded dispatch - void setIslandDispatchFunction( IslandDispatchFunc func ) - { - m_islandDispatch = func; - } + int getMinimumSolverBatchSize() const + { + return m_minimumSolverBatchSize; + } + void setMinimumSolverBatchSize(int sz) + { + m_minimumSolverBatchSize = sz; + } + IslandDispatchFunc getIslandDispatchFunction() const + { + return m_islandDispatch; + } + // allow users to set their own dispatch function for multithreaded dispatch + void setIslandDispatchFunction(IslandDispatchFunc func) + { + m_islandDispatch = func; + } }; - -#endif //BT_SIMULATION_ISLAND_MANAGER_H - +#endif //BT_SIMULATION_ISLAND_MANAGER_H |