diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp | 1052 |
1 files changed, 502 insertions, 550 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 361a054ec69..fb15ae31eb5 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/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,13 +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); - BT_PROFILE("stepSimulation"); - int numSimulationSubSteps = 0; if (maxSubSteps) @@ -413,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; @@ -426,7 +406,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, { numSimulationSubSteps = 0; maxSubSteps = 0; - } else + } + else { numSimulationSubSteps = 1; maxSubSteps = 1; @@ -436,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(); } @@ -466,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); } @@ -489,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()); @@ -514,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,short int collisionFilterGroup,short 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) @@ -553,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); } @@ -572,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()); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(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, short group, short 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); } @@ -598,31 +573,29 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short { 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) @@ -634,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); @@ -667,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()) @@ -776,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()); } } } @@ -784,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,11 +800,22 @@ public: ///don't do CCD when the collision filters are not matching if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; + if (m_pairCache->getOverlapFilterCallback()) { + btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle(); + bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1); + if (!collides) + { + return false; + } + } - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; + + 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) @@ -870,41 +841,24 @@ public: return false; } - - }; ///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; +int gNumClampedCcdMotions = 0; - -void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { - BT_PROFILE("createPredictiveContacts"); - - { - 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(); - } - btTransform predictedTrans; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < numBodies; i++) { - btRigidBody* body = m_nonStaticRigidBodies[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) { @@ -916,81 +870,97 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) 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); + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject); + btMutexLock(&m_predictiveManifoldsMutex); m_predictiveManifolds.push_back(manifold); + 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 = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject); + pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject); pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); pt.m_positionWorldOnB = worldPointB; - } } } } } } -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + +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(); +} + +void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +{ + BT_PROFILE("createPredictiveContacts"); + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } +} + +void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { - BT_PROFILE("integrateTransforms"); btTransform predictedTrans; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < numBodies; i++) { - btRigidBody* body = m_nonStaticRigidBodies[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) { @@ -1002,43 +972,40 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) 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(); @@ -1065,40 +1032,45 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) //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); } ///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 = btManifoldResult::calculateCombinedRestitution(body0, body1); + 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(); @@ -1107,25 +1079,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()) @@ -1134,179 +1100,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)); + 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)); - - 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: { @@ -1324,9 +1282,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) 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)); + if (minTh <= maxTh) + { + 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); @@ -1335,9 +1296,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; @@ -1359,42 +1320,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; @@ -1406,8 +1363,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() return m_constraintSolver; } - -int btDiscreteDynamicsWorld::getNumConstraints() const +int btDiscreteDynamicsWorld::getNumConstraints() const { return int(m_constraints.size()); } @@ -1420,90 +1376,85 @@ 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; - + 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; + + #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); @@ -1512,6 +1463,7 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) serializeRigidBodies(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } - |