diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/Dynamics')
12 files changed, 0 insertions, 4760 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btActionInterface.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btActionInterface.h deleted file mode 100644 index e1fea3a49c..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btActionInterface.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef _BT_ACTION_INTERFACE_H -#define _BT_ACTION_INTERFACE_H - -class btIDebugDraw; -class btCollisionWorld; - -#include "LinearMath/btScalar.h" -#include "btRigidBody.h" - -///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld -class btActionInterface -{ -protected: - - static btRigidBody& getFixedBody(); - - -public: - - virtual ~btActionInterface() - { - } - - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0; - - virtual void debugDraw(btIDebugDraw* debugDrawer) = 0; - -}; - -#endif //_BT_ACTION_INTERFACE_H - diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp deleted file mode 100644 index a196d4522e..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ /dev/null @@ -1,1538 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btDiscreteDynamicsWorld.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btQuickprof.h" - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" -#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" - -#if 0 -btAlignedObjectArray<btVector3> debugContacts; -btAlignedObjectArray<btVector3> debugNormals; -int startHit=2; -int firstHit=startHit; -#endif - -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(); - 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; - } -}; - -struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback -{ - 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, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_sortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - 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); - } - - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - 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 - { - //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++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - if (m_solverInfo->m_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - - for (i=0;i<numBodies;i++) - m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) - m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) - m_constraints.push_back(startConstraint[i]); - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - 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; - - 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) - -{ - if (!m_constraintSolver) - { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; - m_ownsConstraintSolver = true; - } else - { - m_ownsConstraintSolver = false; - } - - { - 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); - } -} - - -btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() -{ - //only delete it when we created it - if (m_ownsIslandManager) - { - m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); - } - if (m_solverIslandCallback) - { - m_solverIslandCallback->~InplaceSolverIslandCallback(); - btAlignedFree(m_solverIslandCallback); - } - if (m_ownsConstraintSolver) - { - - m_constraintSolver->~btConstraintSolver(); - btAlignedFree(m_constraintSolver); - } -} - -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++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getActivationState() != ISLAND_SLEEPING) - { - if (body->isKinematicObject()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } - } - -} - -void btDiscreteDynamicsWorld::debugDrawWorld() -{ - BT_PROFILE("debugDrawWorld"); - - btCollisionWorld::debugDrawWorld(); - - bool drawConstraints = false; - if (getDebugDrawer()) - { - int mode = getDebugDrawer()->getDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) - { - drawConstraints = true; - } - } - if(drawConstraints) - { - 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))) - { - int i; - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) - { - for (i=0;i<m_actions.size();i++) - { - m_actions[i]->debugDraw(m_debugDrawer); - } - } - } - if (getDebugDrawer()) - getDebugDrawer()->flushLines(); - -} - -void btDiscreteDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - //need to check if next line is ok - //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up - body->clearForces(); - } -} - -///apply gravity, call this once per timestep -void btDiscreteDynamicsWorld::applyGravity() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - { - body->applyGravity(); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) -{ - btAssert(body); - - if (body->getMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - ///@todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), - (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), - interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeMotionStates() -{ -// BT_PROFILE("synchronizeMotionStates"); - if (m_synchronizeAllMotionStates) - { - //iterate over all collision objects - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - synchronizeSingleMotionState(body); - } - } else - { - //iterate over all active rigid bodies - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - synchronizeSingleMotionState(body); - } - } -} - - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - startProfiling(timeStep); - - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_fixedTimeStep = fixedTimeStep; - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; - m_fixedTimeStep = 0; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (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; - - saveKinematicState(fixedTimeStep*clampedSimulationSteps); - - applyGravity(); - - - - for (int i=0;i<clampedSimulationSteps;i++) - { - internalSingleStepSimulation(fixedTimeStep); - synchronizeMotionStates(); - } - - } else - { - synchronizeMotionStates(); - } - - clearForces(); - -#ifndef BT_NO_PROFILE - CProfileManager::Increment_Frame_Counter(); -#endif //BT_NO_PROFILE - - return numSimulationSubSteps; -} - -void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) -{ - - BT_PROFILE("internalSingleStepSimulation"); - - if(0 != m_internalPreTickCallback) { - (*m_internalPreTickCallback)(this, timeStep); - } - - ///apply gravity, predict motion - predictUnconstraintMotion(timeStep); - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = getDebugDrawer(); - - - createPredictiveContacts(timeStep); - - ///perform collision detection - performDiscreteCollisionDetection(); - - calculateSimulationIslands(); - - - getSolverInfo().m_timeStep = timeStep; - - - - ///solve contact and other joint constraints - solveConstraints(getSolverInfo()); - - ///CallbackTriggers(); - - ///integrate transforms - - integrateTransforms(timeStep); - - ///update vehicle simulation - updateActions(timeStep); - - updateActivationState( timeStep ); - - if(0 != m_internalTickCallback) { - (*m_internalTickCallback)(this, timeStep); - } -} - -void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) -{ - m_gravity = gravity; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(gravity); - } - } -} - -btVector3 btDiscreteDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) -{ - btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); -} - -void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btRigidBody* body = btRigidBody::upcast(collisionObject); - if (body) - removeRigidBody(body); - else - btCollisionWorld::removeCollisionObject(collisionObject); -} - -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - m_nonStaticRigidBodies.remove(body); - btCollisionWorld::removeCollisionObject(body); -} - - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) -{ - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - if (!body->isStaticObject()) - { - m_nonStaticRigidBodies.push_back(body); - } 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); - - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); - } -} - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) -{ - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - if (!body->isStaticObject()) - { - m_nonStaticRigidBodies.push_back(body); - } - else - { - body->setActivationState(ISLAND_SLEEPING); - } - addCollisionObject(body,group,mask); - } -} - - -void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) -{ - BT_PROFILE("updateActions"); - - for ( int i=0;i<m_actions.size();i++) - { - m_actions[i]->updateAction( this, timeStep); - } -} - - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("updateActivationState"); - - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body) - { - body->updateDeactivation(timeStep); - - if (body->wantsSleeping()) - { - if (body->isStaticOrKinematicObject()) - { - body->setActivationState(ISLAND_SLEEPING); - } else - { - if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); - if (body->getActivationState() == ISLAND_SLEEPING) - { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); - } - - } - } else - { - if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); - } - } - } -} - -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()); - - if (disableCollisionsBetweenLinkedBodies) - { - constraint->getRigidBodyA().addConstraintRef(constraint); - constraint->getRigidBodyB().addConstraintRef(constraint); - } -} - -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) -{ - m_constraints.remove(constraint); - constraint->getRigidBodyA().removeConstraintRef(constraint); - constraint->getRigidBodyB().removeConstraintRef(constraint); -} - -void btDiscreteDynamicsWorld::addAction(btActionInterface* action) -{ - m_actions.push_back(action); -} - -void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) -{ - m_actions.remove(action); -} - - -void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) -{ - addAction(vehicle); -} - -void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) -{ - removeAction(vehicle); -} - -void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) -{ - addAction(character); -} - -void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) -{ - removeAction(character); -} - - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;i<getNumConstraints();i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - -// btAssert(0); - - - - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); - - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - 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_solverIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); -} - - -void btDiscreteDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - 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]; - - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); - - 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++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - if (constraint->isEnabled()) - { - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - } - - //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) - { - } - - 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()) - return 1.0f; - - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); - - 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) - return 1.f; - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - //don't collide with itself - if (proxy0->m_clientObject == m_me) - return false; - - ///don't do CCD when the collision filters are not matching - if (!ClosestConvexResultCallback::needsCollision(proxy0)) - return false; - - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; - - //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 - if (m_dispatcher->needsResponse(m_me,otherObj)) - { -#if 0 - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray<btPersistentManifold*> manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) - { - if (collisionPair->m_algorithm) - { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;j<manifoldArray.size();j++) - { - btPersistentManifold* manifold = manifoldArray[j]; - if (manifold->getNumContacts()>0) - return false; - } - } - } -#endif - return true; - } - - return false; - } - - -}; - -///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; - - -void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep) -{ - btTransform predictedTrans; - 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(); - - if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("predictive convexSweepTest"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; -#ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY - class StaticOnlyCallback : public btClosestNotMeConvexResultCallback - { - public: - - 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 - { - 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()); -#else - 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; - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - btTransform modifiedPredictedTrans = predictedTrans; - modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - - 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 ); - m_predictiveManifolds.push_back(manifold); - btMutexUnlock( &m_predictiveManifoldsMutex ); - - 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); - - 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_positionWorldOnA = body->getWorldTransform().getOrigin(); - pt.m_positionWorldOnB = worldPointB; - - } - } - } - } - } -} - -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 ) -{ - btTransform predictedTrans; - 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(); - - - - if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("CCD motion clamping"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; -#ifdef USE_STATIC_ONLY - class StaticOnlyCallback : public btClosestNotMeConvexResultCallback - { - public: - - 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 - { - 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()); -#else - 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; - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - btTransform modifiedPredictedTrans = predictedTrans; - modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - - 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->setHitFraction(0.f); - body->proceedToTransform( predictedTrans); - -#if 0 - btVector3 linVel = body->getLinearVelocity(); - - btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep; - btScalar maxSpeedSqr = maxSpeed*maxSpeed; - if (linVel.length2()>maxSpeedSqr) - { - linVel.normalize(); - linVel*= maxSpeed; - body->setLinearVelocity(linVel); - btScalar ms2 = body->getLinearVelocity().length2(); - body->predictIntegratedTransform(timeStep, predictedTrans); - - btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - btScalar smt = body->getCcdSquareMotionThreshold(); - printf("sm2=%f\n",sm2); - } -#else - - //don't apply the collision response right now, it will happen next frame - //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution. - //btScalar appliedImpulse = 0.f; - //btScalar depth = 0.f; - //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); - - -#endif - - continue; - } - } - } - - - 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++) - { - 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++) - { - const btManifoldPoint& pt = manifold->getContactPoint(p); - btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1); - - 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; - - const btVector3& pos1 = pt.getPositionWorldOnA(); - const btVector3& pos2 = pt.getPositionWorldOnB(); - - btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); - btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); - - if (body0) - body0->applyImpulse(imp,rel_pos0); - if (body1) - body1->applyImpulse(-imp,rel_pos1); - } - } - } - } -} - - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[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 btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) -{ - (void)timeStep; - -#ifndef BT_NO_PROFILE - CProfileManager::Reset(); -#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)) - { - return; - } - - 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) - { - 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); - } - } - 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) - { - //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)); - - 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; - 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) - { - 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)); - } - } - 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: - { - { - btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint; - btTransform tr = p6DOF->getCalculatedTransformA(); - if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = p6DOF->getCalculatedTransformB(); - if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if (drawLimits) - { - 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)); - } - } - 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) - { - 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; - } - return; -} - - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree( m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; - m_solverIslandCallback->m_solver = solver; -} - -btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} - - -int btDiscreteDynamicsWorld::getNumConstraints() const -{ - return int(m_constraints.size()); -} -btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) -{ - return m_constraints[index]; -} -const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const -{ - return m_constraints[index]; -} - - - -void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) -{ - int i; - //serialize all collision objects - 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); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); - } - } - - 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); - } -} - - - - -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)); - -#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); -} - -void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeDynamicsWorldInfo(serializer); - - serializeCollisionObjects(serializer); - - serializeRigidBodies(serializer); - - serializer->finishSerialization(); -} - diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h deleted file mode 100644 index b0d19f48a3..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ /dev/null @@ -1,239 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -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 - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; -class btSimulationIslandManager; -class btTypedConstraint; -class btActionInterface; -class btPersistentManifold; -class btIDebugDraw; -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 -{ -protected: - - btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints; - InplaceSolverIslandCallback* m_solverIslandCallback; - - btConstraintSolver* m_constraintSolver; - - btSimulationIslandManager* m_islandManager; - - btAlignedObjectArray<btTypedConstraint*> m_constraints; - - btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies; - - btVector3 m_gravity; - - //for variable timesteps - btScalar m_localTime; - btScalar m_fixedTimeStep; - //for variable timesteps - - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - bool m_synchronizeAllMotionStates; - bool m_applySpeculativeContactRestitution; - - btAlignedObjectArray<btActionInterface*> m_actions; - - int m_profileTimings; - - bool m_latencyMotionStateInterpolation; - - btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds; - btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts - - 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(); - - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void updateActivationState(btScalar timeStep); - - void updateActions(btScalar timeStep); - - void startProfiling(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - 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); - - 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 void synchronizeMotionStates(); - - ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - - virtual void removeConstraint(btTypedConstraint* constraint); - - virtual void addAction(btActionInterface*); - - virtual void removeAction(btActionInterface*); - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - btCollisionWorld* getCollisionWorld() - { - return this; - } - - virtual void setGravity(const btVector3& gravity); - - virtual btVector3 getGravity () const; - - 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, int group, int mask); - - 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 debugDrawConstraint(btTypedConstraint* constraint); - - virtual void debugDrawWorld(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - - virtual btTypedConstraint* getConstraint(int index) ; - - virtual const btTypedConstraint* getConstraint(int index) 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(); - - ///apply gravity, call this once per timestep - virtual void applyGravity(); - - virtual void setNumTasks(int numTasks) - { - (void) numTasks; - } - - ///obsolete, use updateActions instead - virtual void updateVehicles(btScalar timeStep) - { - updateActions(timeStep); - } - - ///obsolete, use addAction instead - virtual void addVehicle(btActionInterface* vehicle); - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle); - ///obsolete, use addAction instead - virtual void addCharacter(btActionInterface* character); - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character); - - void setSynchronizeAllMotionStates(bool synchronizeAll) - { - m_synchronizeAllMotionStates = synchronizeAll; - } - bool getSynchronizeAllMotionStates() const - { - return m_synchronizeAllMotionStates; - } - - void setApplySpeculativeContactRestitution(bool enable) - { - 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); - - ///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 ) - { - m_latencyMotionStateInterpolation = latencyInterpolation; - } - bool getLatencyMotionStateInterpolation() const - { - return m_latencyMotionStateInterpolation; - } -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp deleted file mode 100644 index 1d10bad922..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp +++ /dev/null @@ -1,327 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btDiscreteDynamicsWorldMt.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "btSimulationIslandManagerMt.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btQuickprof.h" - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" -#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" - - -struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback -{ - btContactSolverInfo* m_solverInfo; - btConstraintSolver* m_solver; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - InplaceSolverIslandCallbackMt( - btConstraintSolver* solver, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - m_debugDrawer = debugDrawer; - } - - - virtual void processIsland( btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifolds, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - int islandId - ) - { - m_solver->solveGroup( bodies, - numBodies, - manifolds, - numManifolds, - constraints, - numConstraints, - *m_solverInfo, - m_debugDrawer, - m_dispatcher - ); - } - -}; - - -/// -/// btConstraintSolverPoolMt -/// - -btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver() -{ - 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; -} - -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(); - } -} - -// create the solvers for me -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 ); -} - -// pass in fully constructed solvers (destructor will delete them) -btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int 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; - } -} - -///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 -) -{ - 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(); - } -} - - -/// -/// btDiscreteDynamicsWorldMt -/// - -btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration) -: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) -{ - if (m_ownsIslandManager) - { - m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); - } - { - void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16); - m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher); - } - { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16); - btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt(); - im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize ); - m_islandManager = im; - } -} - - -btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt() -{ - if (m_solverIslandCallbackMt) - { - m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt(); - btAlignedFree(m_solverIslandCallbackMt); - } - if (m_ownsConstraintSolver) - { - m_constraintSolver->~btConstraintSolver(); - btAlignedFree(m_constraintSolver); - } -} - - -void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo) -{ - BT_PROFILE("solveConstraints"); - - m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager); - im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt ); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); -} - - -struct UpdaterUnconstrainedMotion : public btIParallelForBody -{ - 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 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 ); - } -} - - -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 ); - } -} - - -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 ); - } -} - diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h deleted file mode 100644 index 2f144cdda4..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h +++ /dev/null @@ -1,134 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -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 - -#include "btDiscreteDynamicsWorld.h" -#include "btSimulationIslandManagerMt.h" -#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" - -struct InplaceSolverIslandCallbackMt; - -/// -/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them. -/// -/// Each solver in the pool is protected by a mutex. When solveGroup is called from a thread, -/// the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the -/// call to the solver. -/// So long as there are at least as many solvers as there are hardware threads, it should never need to -/// spin wait. -/// -class btConstraintSolverPoolMt : public btConstraintSolver -{ -public: - // create the solvers for me - explicit btConstraintSolverPoolMt( int numSolvers ); - - // pass in fully constructed solvers (destructor will delete them) - btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers ); - - 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; - - 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 ); -}; - - - -/// -/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support -/// solving simulation islands on multiple threads. -/// -/// Should function exactly like btDiscreteDynamicsWorld. -/// Also 3 methods that iterate over all of the rigidbodies can run in parallel: -/// - predictUnconstraintMotion -/// - integrateTransforms -/// - createPredictiveContacts -/// -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld -{ -protected: - InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt; - - 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 - btCollisionConfiguration* collisionConfiguration - ); - virtual ~btDiscreteDynamicsWorldMt(); -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h deleted file mode 100644 index 42d8fc0de3..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ /dev/null @@ -1,173 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DYNAMICS_WORLD_H -#define BT_DYNAMICS_WORLD_H - -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class btTypedConstraint; -class btActionInterface; -class btConstraintSolver; -class btDynamicsWorld; - - -/// Type for the callback for each tick -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 -}; - -///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; - - btContactSolverInfo m_solverInfo; - -public: - - - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0) - { - } - - 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; - } - - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} - - virtual void addAction(btActionInterface* action) = 0; - - virtual void removeAction(btActionInterface* action) = 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 synchronizeMotionStates() = 0; - - virtual void addRigidBody(btRigidBody* body) = 0; - - virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; - - virtual void removeRigidBody(btRigidBody* body) = 0; - - virtual void setConstraintSolver(btConstraintSolver* solver) = 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 btDynamicsWorldType getWorldType() const=0; - - virtual void clearForces() = 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; - } - - 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; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btDynamicsWorldFloatData -{ - btContactSolverInfoFloatData m_solverInfo; - btVector3FloatData m_gravity; -}; - - -#endif //BT_DYNAMICS_WORLD_H - - diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp deleted file mode 100644 index ca0714fcfa..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ /dev/null @@ -1,527 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btRigidBody.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMotionState.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "LinearMath/btSerializer.h" - -//'temporarily' global variables -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) -{ - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); - setupRigidBody(cinfo); -} - -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) -{ - - 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_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); - - m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; - m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; - m_optionalMotionState = constructionInfo.m_motionState; - m_contactSolverType = 0; - m_frictionSolverType = 0; - m_additionalDamping = constructionInfo.m_additionalDamping; - m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor; - m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr; - m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr; - m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor; - - if (m_optionalMotionState) - { - m_optionalMotionState->getWorldTransform(m_worldTransform); - } else - { - m_worldTransform = constructionInfo.m_startWorldTransform; - } - - m_interpolationWorldTransform = m_worldTransform; - 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_restitution = constructionInfo.m_restitution; - - 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_pushVelocity.setZero(); - m_turnVelocity.setZero(); - - - -} - - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) -{ - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); -} - -void btRigidBody::saveKinematicState(btScalar timeStep) -{ - //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (timeStep != btScalar(0.)) - { - //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); - 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 -{ - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); -} - - - - -void btRigidBody::setGravity(const btVector3& acceleration) -{ - if (m_inverseMass != btScalar(0.0)) - { - m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); - } - 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) -{ - //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 - -//#define USE_OLD_DAMPING_METHOD 1 -#ifdef USE_OLD_DAMPING_METHOD - 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); -#endif - - if (m_additionalDamping) - { - //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 - if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) && - (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr)) - { - m_angularVelocity *= m_additionalDampingFactor; - m_linearVelocity *= m_additionalDampingFactor; - } - - - btScalar speed = m_linearVelocity.length(); - if (speed < m_linearDamping) - { - btScalar dampVel = btScalar(0.005); - if (speed > dampVel) - { - btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else - { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - - btScalar angSpeed = m_angularVelocity.length(); - if (angSpeed < m_angularDamping) - { - btScalar angDampVel = btScalar(0.005); - if (angSpeed > angDampVel) - { - btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else - { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - } -} - - -void btRigidBody::applyGravity() -{ - if (isStaticOrKinematicObject()) - return; - - applyCentralForce(m_gravity); - -} - -void btRigidBody::proceedToTransform(const btTransform& newTrans) -{ - setCenterOfMassTransform( newTrans ); -} - - -void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) -{ - if (mass == btScalar(0.)) - { - m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; - m_inverseMass = btScalar(0.); - } else - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - m_inverseMass = btScalar(1.0) / mass; - } - - //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; -} - - -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)); - return inertiaLocal; -} - -inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, - const btMatrix3x3 &I) -{ - 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) -{ - - btMatrix3x3 w1x, Iw1x; - 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; - return dfw1; -} - -btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const -{ - btVector3 inertiaLocal = getLocalInertia(); - btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); - btVector3 gf = getAngularVelocity().cross(tmp); - btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*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; - - // Residual vector - btVector3 f = step * omegab.cross(ibo); - - btMatrix3x3 skew0; - omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); - btVector3 om = Ib*omegab; - btMatrix3x3 skew1; - 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; - btVector3 omega_div = J.solve33(f); - - // Single Newton-Raphson update - omegab = omegab - omega_div;//Solve33(J, f); - // Back to world coordinates - 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. - // calculate using implicit euler step so it's stable. - - const btVector3 inertiaLocal = getLocalInertia(); - const btVector3 w0 = getAngularVelocity(); - - btMatrix3x3 I; - - I = m_worldTransform.getBasis().scaled(inertiaLocal) * - 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 - // df/dw' = I + 1xIw'*step + w'xI*step - - btVector3 w1 = w0; - - // one step of newton's method - { - const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I); - const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I); - - btVector3 dw; - dw = dfw.solve33(fw); - //const btMatrix3x3 dfw_inv = dfw.inverse(); - //dw = dfw_inv*fw; - - w1 -= dw; - } - - btVector3 gf = (w1 - w0); - return gf; -} - - -void btRigidBody::integrateVelocities(btScalar step) -{ - if (isStaticOrKinematicObject()) - return; - - m_linearVelocity += m_totalForce * (m_inverseMass * 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 - btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) - { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; - } - -} - -btQuaternion btRigidBody::getOrientation() const -{ - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; -} - - -void btRigidBody::setCenterOfMassTransform(const btTransform& xform) -{ - - if (isKinematicObject()) - { - m_interpolationWorldTransform = m_worldTransform; - } else - { - m_interpolationWorldTransform = xform; - } - m_interpolationLinearVelocity = getLinearVelocity(); - m_interpolationAngularVelocity = getAngularVelocity(); - m_worldTransform = xform; - updateInertiaTensor(); -} - - - - - -void btRigidBody::addConstraintRef(btTypedConstraint* c) -{ - ///disable collision with the 'other' body - - int index = m_constraintRefs.findLinearSearch(c); - //don't add constraints that are already referenced - //btAssert(index == m_constraintRefs.size()); - if (index == m_constraintRefs.size()) - { - m_constraintRefs.push_back(c); - btCollisionObject* colObjA = &c->getRigidBodyA(); - btCollisionObject* colObjB = &c->getRigidBodyB(); - if (colObjA == this) - { - colObjA->setIgnoreCollisionCheck(colObjB, true); - } - else - { - 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); - } - } -} - -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 -{ - btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; - - btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); - - m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld); - m_linearVelocity.serialize(rbd->m_linearVelocity); - m_angularVelocity.serialize(rbd->m_angularVelocity); - rbd->m_inverseMass = m_inverseMass; - m_angularFactor.serialize(rbd->m_angularFactor); - m_linearFactor.serialize(rbd->m_linearFactor); - m_gravity.serialize(rbd->m_gravity); - m_gravity_acceleration.serialize(rbd->m_gravity_acceleration); - m_invInertiaLocal.serialize(rbd->m_invInertiaLocal); - m_totalForce.serialize(rbd->m_totalForce); - m_totalTorque.serialize(rbd->m_totalTorque); - rbd->m_linearDamping = m_linearDamping; - rbd->m_angularDamping = m_angularDamping; - rbd->m_additionalDamping = m_additionalDamping; - rbd->m_additionalDampingFactor = m_additionalDampingFactor; - rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; - rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; - rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; - rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; - rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; - - // Fill padding with zeros to appease msan. -#ifdef BT_USE_DOUBLE_PRECISION - memset(rbd->m_padding, 0, sizeof(rbd->m_padding)); -#endif - - return btRigidBodyDataName; -} - - - -void btRigidBody::serializeSingleObject(class btSerializer* serializer) const -{ - btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); - const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); -} - - diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btRigidBody.h deleted file mode 100644 index 372245031b..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btRigidBody.h +++ /dev/null @@ -1,619 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_RIGIDBODY_H -#define BT_RIGIDBODY_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btTransform.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -class btCollisionShape; -class btMotionState; -class btTypedConstraint; - - -extern btScalar gDeactivationTime; -extern bool gDisableDeactivation; - -#ifdef BT_USE_DOUBLE_PRECISION -#define btRigidBodyData btRigidBodyDoubleData -#define btRigidBodyDataName "btRigidBodyDoubleData" -#else -#define btRigidBodyData btRigidBodyFloatData -#define btRigidBodyDataName "btRigidBodyFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - -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_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: -///- 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. -///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 -{ - - 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; - - //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; - - -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. - ///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 - { - 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; - - btCollisionShape* m_collisionShape; - btVector3 m_localInertia; - btScalar m_linearDamping; - btScalar m_angularDamping; - - ///best simulation results when friction is non-zero - 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 - - ///best simulation results using zero restitution. - btScalar m_restitution; - - 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)) - { - m_startWorldTransform.setIdentity(); - } - }; - - ///btRigidBody constructor using construction info - btRigidBody( const btRigidBodyConstructionInfo& constructionInfo); - - ///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)); - - - 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); - } - -protected: - - ///setupRigidBody is only used internally by the constructor - void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); - -public: - - 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) - { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) - return (const btRigidBody*)colObj; - return 0; - } - static btRigidBody* upcast(btCollisionObject* colObj) - { - 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); - - const btVector3& getGravity() const - { - return m_gravity_acceleration; - } - - void setDamping(btScalar lin_damping, btScalar ang_damping); - - btScalar getLinearDamping() const - { - return m_linearDamping; - } - - btScalar getAngularDamping() const - { - return m_angularDamping; - } - - btScalar getLinearSleepingThreshold() const - { - return m_linearSleepingThreshold; - } - - btScalar getAngularSleepingThreshold() const - { - return m_angularSleepingThreshold; - } - - void applyDamping(btScalar timeStep); - - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { - return m_collisionShape; - } - - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { - return m_collisionShape; - } - - void setMassProps(btScalar mass, const btVector3& inertia); - - const btVector3& getLinearFactor() const - { - return m_linearFactor; - } - void setLinearFactor(const btVector3& linearFactor) - { - m_linearFactor = linearFactor; - m_invMass = m_linearFactor*m_inverseMass; - } - btScalar getInvMass() const { return m_inverseMass; } - const btMatrix3x3& getInvInertiaTensorWorld() const { - return m_invInertiaTensorWorld; - } - - void integrateVelocities(btScalar step); - - void setCenterOfMassTransform(const btTransform& xform); - - void applyCentralForce(const btVector3& force) - { - m_totalForce += force*m_linearFactor; - } - - const btVector3& getTotalForce() const - { - return m_totalForce; - }; - - const btVector3& getTotalTorque() const - { - return m_totalTorque; - }; - - const btVector3& getInvInertiaDiagLocal() const - { - return m_invInertiaLocal; - }; - - void setInvInertiaDiagLocal(const btVector3& diagInvInertia) - { - m_invInertiaLocal = diagInvInertia; - } - - void setSleepingThresholds(btScalar linear,btScalar angular) - { - m_linearSleepingThreshold = linear; - m_angularSleepingThreshold = angular; - } - - void applyTorque(const btVector3& torque) - { - m_totalTorque += torque*m_angularFactor; - } - - void applyForce(const btVector3& force, const btVector3& rel_pos) - { - applyCentralForce(force); - applyTorque(rel_pos.cross(force*m_linearFactor)); - } - - void applyCentralImpulse(const btVector3& impulse) - { - m_linearVelocity += impulse *m_linearFactor * m_inverseMass; - } - - void applyTorqueImpulse(const btVector3& torque) - { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; - } - - 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)); - } - } - } - - 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(); - } - btQuaternion getOrientation() const; - - const btTransform& getCenterOfMassTransform() const { - return m_worldTransform; - } - const btVector3& getLinearVelocity() const { - return m_linearVelocity; - } - const btVector3& getAngularVelocity() const { - return m_angularVelocity; - } - - - inline void setLinearVelocity(const btVector3& lin_vel) - { - m_updateRevision++; - m_linearVelocity = lin_vel; - } - - inline void setAngularVelocity(const btVector3& ang_vel) - { - m_updateRevision++; - m_angularVelocity = ang_vel; - } - - btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const - { - //we also calculate lin/ang velocity for kinematic objects - return m_linearVelocity + m_angularVelocity.cross(rel_pos); - - //for kinematic objects, we could also use use: - // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; - } - - void translate(const btVector3& v) - { - m_worldTransform.getOrigin() += v; - } - - - void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; - - - - - - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const - { - btVector3 r0 = pos - getCenterOfMassPosition(); - - btVector3 c0 = (r0).cross(normal); - - btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); - - return m_inverseMass + normal.dot(vec); - - } - - SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const - { - btVector3 vec = axis * getInvInertiaTensorWorld(); - return axis.dot(vec); - } - - SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) - { - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) - return; - - if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && - (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) - { - m_deactivationTime += timeStep; - } else - { - m_deactivationTime=btScalar(0.); - setActivationState(0); - } - - } - - SIMD_FORCE_INLINE bool wantsSleeping() - { - - if (getActivationState() == DISABLE_DEACTIVATION) - return false; - - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) - return false; - - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) - return true; - - if (m_deactivationTime> gDeactivationTime) - { - return true; - } - return false; - } - - - - const btBroadphaseProxy* getBroadphaseProxy() const - { - return m_broadphaseHandle; - } - btBroadphaseProxy* getBroadphaseProxy() - { - return m_broadphaseHandle; - } - void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) - { - m_broadphaseHandle = broadphaseProxy; - } - - //btMotionState allows to automatic synchronize the world transform for active objects - btMotionState* getMotionState() - { - return m_optionalMotionState; - } - const btMotionState* getMotionState() const - { - return m_optionalMotionState; - } - void setMotionState(btMotionState* motionState) - { - m_optionalMotionState = motionState; - if (m_optionalMotionState) - motionState->getWorldTransform(m_worldTransform); - } - - //for experimental overriding of friction/contact solver func - int m_contactSolverType; - int m_frictionSolverType; - - void setAngularFactor(const btVector3& angFac) - { - m_updateRevision++; - m_angularFactor = angFac; - } - - void setAngularFactor(btScalar angFac) - { - m_updateRevision++; - m_angularFactor.setValue(angFac,angFac,angFac); - } - const btVector3& getAngularFactor() const - { - return m_angularFactor; - } - - //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - bool isInWorld() const - { - return (getBroadphaseProxy() != 0); - } - - void addConstraintRef(btTypedConstraint* c); - void removeConstraintRef(btTypedConstraint* c); - - btTypedConstraint* getConstraintRef(int index) - { - return m_constraintRefs[index]; - } - - int getNumConstraintRefs() const - { - return m_constraintRefs.size(); - } - - void setFlags(int flags) - { - m_rigidbodyFlags = flags; - } - - int getFlags() const - { - 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; - - ///explicit version is best avoided, it gains energy - btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const; - btVector3 getLocalInertia() 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 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 -{ - 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 -{ - 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 - diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp deleted file mode 100644 index 6f63b87c80..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ /dev/null @@ -1,280 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSimpleDynamicsWorld.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#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" -{ - 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() -{ - if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); -} - -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - (void)fixedTimeStep; - (void)maxSubSteps; - - - ///apply gravity, predict motion - predictUnconstraintMotion(timeStep); - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = getDebugDrawer(); - - ///perform collision detection - performDiscreteCollisionDetection(); - - ///solve contact constraints - int numManifolds = m_dispatcher1->getNumManifolds(); - 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); - } - - ///integrate transforms - integrateTransforms(timeStep); - - updateAabbs(); - - synchronizeMotionStates(); - - clearForces(); - - return 1; - -} - -void btSimpleDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - 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) -{ - m_gravity = gravity; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - body->setGravity(gravity); - } - } -} - -btVector3 btSimpleDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - btCollisionWorld::removeCollisionObject(body); -} - -void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btRigidBody* body = btRigidBody::upcast(collisionObject); - if (body) - removeRigidBody(body); - else - btCollisionWorld::removeCollisionObject(collisionObject); -} - - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) -{ - body->setGravity(m_gravity); - - if (body->getCollisionShape()) - { - addCollisionObject(body); - } -} - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) -{ - body->setGravity(m_gravity); - - if (body->getCollisionShape()) - { - addCollisionObject(body,group,mask); - } -} - - -void btSimpleDynamicsWorld::debugDrawWorld() -{ - -} - -void btSimpleDynamicsWorld::addAction(btActionInterface* action) -{ - -} - -void btSimpleDynamicsWorld::removeAction(btActionInterface* action) -{ - -} - - -void btSimpleDynamicsWorld::updateAabbs() -{ - btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - if (body->isActive() && (!body->isStaticObject())) - { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } - } - } -} - -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - if (body->isActive() && (!body->isStaticObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); - } - } - } -} - - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - if (!body->isStaticObject()) - { - if (body->isActive()) - { - body->applyGravity(); - body->integrateVelocities( timeStep); - body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } - } - } -} - - -void btSimpleDynamicsWorld::synchronizeMotionStates() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState()) - { - if (body->getActivationState() != ISLAND_SLEEPING) - { - body->getMotionState()->setWorldTransform(body->getWorldTransform()); - } - } - } - -} - - -void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree(m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h deleted file mode 100644 index 44b7e7fb34..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ /dev/null @@ -1,89 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SIMPLE_DYNAMICS_WORLD_H -#define BT_SIMPLE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; - -///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. -///Please use btDiscreteDynamicsWorld instead -class btSimpleDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - bool m_ownsConstraintSolver; - - 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); - - 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 void setGravity(const btVector3& gravity); - - virtual btVector3 getGravity () const; - - virtual void addRigidBody(btRigidBody* body); - - virtual void addRigidBody(btRigidBody* body, int group, int mask); - - virtual void removeRigidBody(btRigidBody* body); - - virtual void debugDrawWorld(); - - 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 synchronizeMotionStates(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual btDynamicsWorldType getWorldType() const - { - return BT_SIMPLE_DYNAMICS_WORLD; - } - - virtual void clearForces(); - -}; - -#endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp deleted file mode 100644 index 99b34353c7..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ /dev/null @@ -1,678 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "LinearMath/btScalar.h" -#include "LinearMath/btThreads.h" -#include "btSimulationIslandManagerMt.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - -//#include <stdio.h> -#include "LinearMath/btQuickprof.h" - - -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; -} - - -SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island ) -{ - 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; -} - - -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 ); -} - - -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(); - return islandId; -} - - -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; -} - -/// 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; - } -}; - - -class IslandBodyCapacitySortPredicate -{ -public: - 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 ) -{ - // 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 ) -{ - 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 ); - } -} - - -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; -} - - -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; -} - - -void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld ) -{ - - BT_PROFILE("islandUnionFindAndQuickSort"); - - 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 startIslandIndex; - - //update the sleeping state for bodies, if all are sleeping - 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++) - { - } - - //int numSleeping = 0; - - bool allSleeping = true; - - int 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"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - if (colObj0->getIslandTag() == islandId) - { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } - } - - if (allSleeping) - { - int 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"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - - if (colObj0->getIslandTag() == islandId) - { - colObj0->setActivationState( ISLAND_SLEEPING ); - } - } - } else - { - - int 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"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - - if (colObj0->getIslandTag() == islandId) - { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) - { - colObj0->setActivationState( WANTS_DEACTIVATION); - colObj0->setDeactivationTime(0.f); - } - } - } - } - } -} - - -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 ); - } - } - } - -} - - -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 ); - } - } - } - } -} - - -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++; - } -} - - -void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback ) -{ - BT_PROFILE( "serialIslandDispatch" ); - // serial dispatch - btAlignedObjectArray<Island*>& islands = *islandsPtr; - for ( int i = 0; i < islands.size(); ++i ) - { - Island* island = islands[ i ]; - btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL; - btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL; - callback->processIsland( &island->bodyArray[ 0 ], - island->bodyArray.size(), - manifolds, - island->manifoldArray.size(), - constraintsPtr, - island->constraintArray.size(), - island->id - ); - } -} - -struct UpdateIslandDispatcher : public btIParallelForBody -{ - btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr; - btSimulationIslandManagerMt::IslandCallback* callback; - - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - for ( int i = iBegin; i < iEnd; ++i ) - { - btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ]; - btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL; - btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL; - callback->processIsland( &island->bodyArray[ 0 ], - island->bodyArray.size(), - manifolds, - island->manifoldArray.size(), - constraintsPtr, - island->constraintArray.size(), - island->id - ); - } - } -}; - -void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback ) -{ - BT_PROFILE( "parallelIslandDispatch" ); - int grainSize = 1; // iterations per task - UpdateIslandDispatcher dispatcher; - dispatcher.islandsPtr = islandsPtr; - dispatcher.callback = callback; - btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher ); -} - - -///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher, - btCollisionWorld* collisionWorld, - btAlignedObjectArray<btTypedConstraint*>& constraints, - IslandCallback* callback - ) -{ - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - buildIslands(dispatcher,collisionWorld); - - BT_PROFILE("processIslands"); - - 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; - callback->processIsland(&collisionObjects[0], - collisionObjects.size(), - manifolds, - maxNumManifolds, - constraintsPtr, - constraints.size(), - -1 - ); - } - 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, callback ); - } -} diff --git a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h deleted file mode 100644 index 9a781aaef1..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h +++ /dev/null @@ -1,110 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SIMULATION_ISLAND_MANAGER_MT_H -#define BT_SIMULATION_ISLAND_MANAGER_MT_H - -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" - -class btTypedConstraint; - - -/// -/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager -/// Splits the world up into islands which can be solved in parallel. -/// In order to solve islands in parallel, an IslandDispatch function -/// must be provided which will dispatch calls to multiple threads. -/// The amount of parallelism that can be achieved depends on the number -/// of islands. If only a single island exists, then no parallelism is -/// possible. -/// -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; - - void append( const Island& other ); // add bodies, manifolds, constraints to my own - }; - struct IslandCallback - { - virtual ~IslandCallback() {}; - - virtual void processIsland( btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifolds, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - int islandId - ) = 0; - }; - typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback ); - static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback ); - static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback ); -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; - - 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, IslandCallback* callback ); - - 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; - } -}; - -#endif //BT_SIMULATION_ISLAND_MANAGER_H - |