diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Dynamics')
12 files changed, 0 insertions, 4666 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h b/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h deleted file mode 100644 index b5cac56cdc..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h +++ /dev/null @@ -1,41 +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/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp deleted file mode 100644 index fb15ae31eb..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ /dev/null @@ -1,1469 +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; - if (m_pairCache->getOverlapFilterCallback()) { - btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle(); - bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1); - if (!collides) - { - return false; - } - } - - btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; - - if (!m_dispatcher->needsCollision(m_me, otherObj)) - return false; - - //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 - if (m_dispatcher->needsResponse(m_me, otherObj)) - { -#if 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; - if (minTh <= maxTh) - { - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); - } - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - 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; - - -#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); - - serializeContactManifolds(serializer); - - serializer->finishSerialization(); -} diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h deleted file mode 100644 index 73607c61fd..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ /dev/null @@ -1,244 +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 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 solveConstraints(btContactSolverInfo & solverInfo); - - 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; - } - - btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() - { - return m_nonStaticRigidBodies; - } - - const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const - { - return m_nonStaticRigidBodies; - } -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp deleted file mode 100644 index 8207b47135..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp +++ /dev/null @@ -1,263 +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" - -/// -/// 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* solverPool, - btConstraintSolver* constraintSolverMt, - btCollisionConfiguration* collisionConfiguration) - : btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration) -{ - if (m_ownsIslandManager) - { - m_islandManager->~btSimulationIslandManager(); - btAlignedFree(m_islandManager); - } - { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16); - btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt(); - im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize); - m_islandManager = im; - } - m_constraintSolverMt = constraintSolverMt; -} - -btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt() -{ -} - -void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo) -{ - BT_PROFILE("solveConstraints"); - - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager); - btSimulationIslandManagerMt::SolverParams solverParams; - solverParams.m_solverPool = m_constraintSolver; - solverParams.m_solverMt = m_constraintSolverMt; - solverParams.m_solverInfo = &solverInfo; - solverParams.m_debugDrawer = m_debugDrawer; - solverParams.m_dispatcher = getCollisionWorld()->getDispatcher(); - im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams); - - 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); - } -} - -int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) -{ - int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep); - if (btITaskScheduler* scheduler = btGetTaskScheduler()) - { - // tell Bullet's threads to sleep, so other threads can run - scheduler->sleepWorkerThreadsHint(); - } - return numSubSteps; -} diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h deleted file mode 100644 index dccf35d7a7..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h +++ /dev/null @@ -1,131 +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" - -/// -/// 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: - btConstraintSolver* m_constraintSolverMt; - - virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE; - - virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE; - - struct UpdaterCreatePredictiveContacts : public btIParallelForBody - { - btScalar timeStep; - btRigidBody** rigidBodies; - btDiscreteDynamicsWorldMt* world; - - void forLoop(int iBegin, int iEnd) const BT_OVERRIDE - { - world->createPredictiveContactsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep); - } - }; - virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE; - - struct UpdaterIntegrateTransforms : public btIParallelForBody - { - btScalar timeStep; - btRigidBody** rigidBodies; - btDiscreteDynamicsWorldMt* world; - - void forLoop(int iBegin, int iEnd) const BT_OVERRIDE - { - world->integrateTransformsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep); - } - }; - virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btDiscreteDynamicsWorldMt(btDispatcher * dispatcher, - btBroadphaseInterface * pairCache, - btConstraintSolverPoolMt * solverPool, // Note this should be a solver-pool for multi-threading - btConstraintSolver * constraintSolverMt, // single multi-threaded solver for large islands (or NULL) - btCollisionConfiguration * collisionConfiguration); - virtual ~btDiscreteDynamicsWorldMt(); - - virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE; -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h deleted file mode 100644 index 3c55234a8a..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h +++ /dev/null @@ -1,174 +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, - BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7 -}; - -///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/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp deleted file mode 100644 index 27fdead761..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp +++ /dev/null @@ -1,505 +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) -{ -#ifdef BT_USE_OLD_DAMPING_METHOD - m_linearDamping = btMax(lin_damping, btScalar(0.0)); - m_angularDamping = btMax(ang_damping, btScalar(0.0)); -#else - m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0)); - m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0)); -#endif -} - -///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 - -#ifdef BT_USE_OLD_DAMPING_METHOD - m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0)); - m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.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::clearGravity() -{ - 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; - } - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_angularVelocity); - #endif -} - -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/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h deleted file mode 100644 index 7442dd1e6a..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h +++ /dev/null @@ -1,687 +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 one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. -///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time. -///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects) -class btRigidBody : public btCollisionObject -{ - 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 clearGravity(); - - 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; } - btScalar getMass() const { return m_inverseMass == btScalar(0.) ? btScalar(0.) : btScalar(1.0) / 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; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_totalTorque); - #endif - } - - 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; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_linearVelocity); - #endif - } - - void applyTorqueImpulse(const btVector3& torque) - { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_angularVelocity); - #endif - } - - 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 applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos) - { - if (m_inverseMass != btScalar(0.)) - { - applyCentralPushImpulse(impulse); - if (m_angularFactor) - { - applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor)); - } - } - } - - btVector3 getPushVelocity() const - { - return m_pushVelocity; - } - - btVector3 getTurnVelocity() const - { - return m_turnVelocity; - } - - void setPushVelocity(const btVector3& v) - { - m_pushVelocity = v; - } - - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - void clampVelocity(btVector3& v) const { - v.setX( - fmax(-BT_CLAMP_VELOCITY_TO, - fmin(BT_CLAMP_VELOCITY_TO, v.getX())) - ); - v.setY( - fmax(-BT_CLAMP_VELOCITY_TO, - fmin(BT_CLAMP_VELOCITY_TO, v.getY())) - ); - v.setZ( - fmax(-BT_CLAMP_VELOCITY_TO, - fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) - ); - } - #endif - - void setTurnVelocity(const btVector3& v) - { - m_turnVelocity = v; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_turnVelocity); - #endif - } - - void applyCentralPushImpulse(const btVector3& impulse) - { - m_pushVelocity += impulse * m_linearFactor * m_inverseMass; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_pushVelocity); - #endif - } - - void applyTorqueTurnImpulse(const btVector3& torque) - { - m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_turnVelocity); - #endif - } - - 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; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_linearVelocity); - #endif - } - - inline void setAngularVelocity(const btVector3& ang_vel) - { - m_updateRevision++; - m_angularVelocity = ang_vel; - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - clampVelocity(m_angularVelocity); - #endif - } - - 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; - } - - btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const - { - //we also calculate lin/ang velocity for kinematic objects - return m_pushVelocity + m_turnVelocity.cross(rel_pos); - } - - 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/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp deleted file mode 100644 index 8103390fb1..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ /dev/null @@ -1,260 +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/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h deleted file mode 100644 index 12be231c7f..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ /dev/null @@ -1,84 +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/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp deleted file mode 100644 index 772b774202..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ /dev/null @@ -1,696 +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 "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching - -//#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 btGetConstraintIslandId1(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) -{ - btAssert(id >= 0); - btAssert(id < m_lookupIslandFromId.size()); - 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("buildIslands"); - - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - //we are going to sort the unionfind array, and store the element id in the size - //afterwards, we clean unionfind, to make sure no-one uses it anymore - - getUnionFind().sortIslands(); - int numElem = getUnionFind().getNumElements(); - - int endIslandIndex = 1; - int 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 || - colObj0->getActivationState() == DISABLE_DEACTIVATION) - { - allSleeping = false; - break; - } - } - } - - 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 = btGetConstraintIslandId1(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::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams) -{ - btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL; - btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL; - solver->solveGroup(&island.bodyArray[0], - island.bodyArray.size(), - manifolds, - island.manifoldArray.size(), - constraintsPtr, - island.constraintArray.size(), - *solverParams.m_solverInfo, - solverParams.m_debugDrawer, - solverParams.m_dispatcher); -} - -void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) -{ - BT_PROFILE("serialIslandDispatch"); - // serial dispatch - btAlignedObjectArray<Island*>& islands = *islandsPtr; - btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; - for (int i = 0; i < islands.size(); ++i) - { - solveIsland(solver, *islands[i], solverParams); - } -} - -struct UpdateIslandDispatcher : public btIParallelForBody -{ - btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr; - const btSimulationIslandManagerMt::SolverParams& m_solverParams; - - UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) - : m_islandsPtr(islandsPtr), m_solverParams(solverParams) - { - } - - void forLoop(int iBegin, int iEnd) const BT_OVERRIDE - { - btConstraintSolver* solver = m_solverParams.m_solverPool; - for (int i = iBegin; i < iEnd; ++i) - { - btSimulationIslandManagerMt::Island* island = m_islandsPtr[i]; - btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams); - } - } -}; - -void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) -{ - BT_PROFILE("parallelIslandDispatch"); - // - // if there are islands with many contacts, it may be faster to submit these - // large islands *serially* to a single parallel constraint solver, and then later - // submit the remaining smaller islands in parallel to multiple sequential solvers. - // - // Some task schedulers do not deal well with nested parallelFor loops. One implementation - // of OpenMP was actually slower than doing everything single-threaded. Intel TBB - // on the other hand, seems to do a pretty respectable job with it. - // - // When solving islands in parallel, the worst case performance happens when there - // is one very large island and then perhaps a smattering of very small - // islands -- one worker thread takes the large island and the remaining workers - // tear through the smaller islands and then sit idle waiting for the first worker - // to finish. Solving islands in parallel works best when there are numerous small - // islands, roughly equal in size. - // - // By contrast, the other approach -- the parallel constraint solver -- is only - // able to deliver a worthwhile speedup when the island is large. For smaller islands, - // it is difficult to extract a useful amount of parallelism -- the overhead of grouping - // the constraints into batches and sending the batches to worker threads can nullify - // any gains from parallelism. - // - - UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); - // We take advantage of the fact the islands are sorted in order of decreasing size - int iBegin = 0; - if (solverParams.m_solverMt) - { - while (iBegin < islandsPtr->size()) - { - btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin]; - if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching) - { - // OK to submit the rest of the array in parallel - break; - } - // serial dispatch to parallel solver for large islands (if any) - solveIsland(solverParams.m_solverMt, *island, solverParams); - ++iBegin; - } - } - // parallel dispatch to sequential solvers for rest - btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher); -} - -///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher, - btCollisionWorld* collisionWorld, - btAlignedObjectArray<btTypedConstraint*>& constraints, - const SolverParams& solverParams) -{ - BT_PROFILE("buildAndProcessIslands"); - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - buildIslands(dispatcher, collisionWorld); - - if (!getSplitIslands()) - { - btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); - int maxNumManifolds = dispatcher->getNumManifolds(); - - for (int i = 0; i < maxNumManifolds; i++) - { - btPersistentManifold* manifold = manifolds[i]; - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); - - ///@todo: check sleeping conditions! - if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || - ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) - { - //kinematic objects don't merge islands, but wake up all connected objects - if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) - { - if (colObj0->hasContactResponse()) - colObj1->activate(); - } - if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) - { - if (colObj1->hasContactResponse()) - colObj0->activate(); - } - } - } - btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL; - btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; - solver->solveGroup(&collisionObjects[0], - collisionObjects.size(), - manifolds, - maxNumManifolds, - constraintsPtr, - constraints.size(), - *solverParams.m_solverInfo, - solverParams.m_debugDrawer, - solverParams.m_dispatcher); - } - else - { - initIslandPools(); - - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - addBodiesToIslands(collisionWorld); - addManifoldsToIslands(dispatcher); - addConstraintsToIslands(constraints); - - // m_activeIslands array should now contain all non-sleeping Islands, and each Island should - // have all the necessary bodies, manifolds and constraints. - - // if we want to merge islands with small batch counts, - if (m_minimumSolverBatchSize > 1) - { - mergeIslands(); - } - // dispatch islands to solver - m_islandDispatch(&m_activeIslands, solverParams); - } -} diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h deleted file mode 100644 index ab73a899f1..0000000000 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h +++ /dev/null @@ -1,112 +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; -class btConstraintSolver; -struct btContactSolverInfo; -class btIDebugDraw; - -/// -/// 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 SolverParams - { - btConstraintSolver* m_solverPool; - btConstraintSolver* m_solverMt; - btContactSolverInfo* m_solverInfo; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - }; - static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams); - - typedef void (*IslandDispatchFunc)(btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams); - static void serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams); - static void parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams); - -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, - const SolverParams& solverParams); - - 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 |