diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp | 1538 |
1 files changed, 1538 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp new file mode 100644 index 0000000000..a196d4522e --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -0,0 +1,1538 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btDiscreteDynamicsWorld.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btQuickprof.h" + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" + + +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + + +#include "BulletDynamics/Dynamics/btActionInterface.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btMotionState.h" + +#include "LinearMath/btSerializer.h" + +#if 0 +btAlignedObjectArray<btVector3> debugContacts; +btAlignedObjectArray<btVector3> debugNormals; +int startHit=2; +int firstHit=startHit; +#endif + +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + btContactSolverInfo* m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + + + InplaceSolverIslandCallback( + btConstraintSolver* solver, + btStackAlloc* stackAlloc, + btDispatcher* dispatcher) + :m_solverInfo(NULL), + m_solver(solver), + m_sortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + + } + + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) + { + btAssert(solverInfo); + m_solverInfo = solverInfo; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + m_debugDrawer = debugDrawer; + m_bodies.resize (0); + m_manifolds.resize (0); + m_constraints.resize (0); + } + + + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + int numCurConstraints = 0; + int i; + + //find the first constraint for this island + for (i=0;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + if (m_solverInfo->m_minimumSolverBatchSize<=1) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + + for (i=0;i<numBodies;i++) + m_bodies.push_back(bodies[i]); + for (i=0;i<numManifolds;i++) + m_manifolds.push_back(manifolds[i]); + for (i=0;i<numCurConstraints;i++) + m_constraints.push_back(startConstraint[i]); + if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } + } + } + } + void processConstraints() + { + + btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; + btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; + btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; + + m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + + } + +}; + + + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) +:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), +m_sortedConstraints (), +m_solverIslandCallback ( NULL ), +m_constraintSolver(constraintSolver), +m_gravity(0,-10,0), +m_localTime(0), +m_fixedTimeStep(0), +m_synchronizeAllMotionStates(false), +m_applySpeculativeContactRestitution(false), +m_profileTimings(0), +m_latencyMotionStateInterpolation(true) + +{ + if (!m_constraintSolver) + { + void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); + m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; + m_ownsConstraintSolver = true; + } else + { + m_ownsConstraintSolver = false; + } + + { + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); + m_islandManager = new (mem) btSimulationIslandManager(); + } + + m_ownsIslandManager = true; + + { + void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16); + m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher); + } +} + + +btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() +{ + //only delete it when we created it + if (m_ownsIslandManager) + { + m_islandManager->~btSimulationIslandManager(); + btAlignedFree( m_islandManager); + } + if (m_solverIslandCallback) + { + m_solverIslandCallback->~InplaceSolverIslandCallback(); + btAlignedFree(m_solverIslandCallback); + } + if (m_ownsConstraintSolver) + { + + m_constraintSolver->~btConstraintSolver(); + btAlignedFree(m_constraintSolver); + } +} + +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) +{ +///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows +///to switch status _after_ adding kinematic objects to the world +///fix it for Bullet 3.x release + for (int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->getActivationState() != ISLAND_SLEEPING) + { + if (body->isKinematicObject()) + { + //to calculate velocities next frame + body->saveKinematicState(timeStep); + } + } + } + +} + +void btDiscreteDynamicsWorld::debugDrawWorld() +{ + BT_PROFILE("debugDrawWorld"); + + btCollisionWorld::debugDrawWorld(); + + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } + } + if(drawConstraints) + { + for(int i = getNumConstraints()-1; i>=0 ;i--) + { + btTypedConstraint* constraint = getConstraint(i); + debugDrawConstraint(constraint); + } + } + + + + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) + { + int i; + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) + { + for (i=0;i<m_actions.size();i++) + { + m_actions[i]->debugDraw(m_debugDrawer); + } + } + } + if (getDebugDrawer()) + getDebugDrawer()->flushLines(); + +} + +void btDiscreteDynamicsWorld::clearForces() +{ + ///@todo: iterate over awake simulation islands! + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + //need to check if next line is ok + //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up + body->clearForces(); + } +} + +///apply gravity, call this once per timestep +void btDiscreteDynamicsWorld::applyGravity() +{ + ///@todo: iterate over awake simulation islands! + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + { + body->applyGravity(); + } + } +} + + +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +{ + btAssert(body); + + if (body->getMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + ///@todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), + (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), + interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } +} + + +void btDiscreteDynamicsWorld::synchronizeMotionStates() +{ +// BT_PROFILE("synchronizeMotionStates"); + if (m_synchronizeAllMotionStates) + { + //iterate over all collision objects + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + synchronizeSingleMotionState(body); + } + } else + { + //iterate over all active rigid bodies + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + synchronizeSingleMotionState(body); + } + } +} + + +int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +{ + startProfiling(timeStep); + + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int( m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer (); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep*clampedSimulationSteps); + + applyGravity(); + + + + for (int i=0;i<clampedSimulationSteps;i++) + { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + + } else + { + synchronizeMotionStates(); + } + + clearForces(); + +#ifndef BT_NO_PROFILE + CProfileManager::Increment_Frame_Counter(); +#endif //BT_NO_PROFILE + + return numSimulationSubSteps; +} + +void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +{ + + BT_PROFILE("internalSingleStepSimulation"); + + if(0 != m_internalPreTickCallback) { + (*m_internalPreTickCallback)(this, timeStep); + } + + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + + + createPredictiveContacts(timeStep); + + ///perform collision detection + performDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + + getSolverInfo().m_timeStep = timeStep; + + + + ///solve contact and other joint constraints + solveConstraints(getSolverInfo()); + + ///CallbackTriggers(); + + ///integrate transforms + + integrateTransforms(timeStep); + + ///update vehicle simulation + updateActions(timeStep); + + updateActivationState( timeStep ); + + if(0 != m_internalTickCallback) { + (*m_internalTickCallback)(this, timeStep); + } +} + +void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) +{ + m_gravity = gravity; + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + { + body->setGravity(gravity); + } + } +} + +btVector3 btDiscreteDynamicsWorld::getGravity () const +{ + return m_gravity; +} + +void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +{ + btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); +} + +void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btRigidBody* body = btRigidBody::upcast(collisionObject); + if (body) + removeRigidBody(body); + else + btCollisionWorld::removeCollisionObject(collisionObject); +} + +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + m_nonStaticRigidBodies.remove(body); + btCollisionWorld::removeCollisionObject(body); +} + + +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +{ + if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + if (!body->isStaticObject()) + { + m_nonStaticRigidBodies.push_back(body); + } else + { + body->setActivationState(ISLAND_SLEEPING); + } + + bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); + int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + } +} + +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) +{ + if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + if (!body->isStaticObject()) + { + m_nonStaticRigidBodies.push_back(body); + } + else + { + body->setActivationState(ISLAND_SLEEPING); + } + addCollisionObject(body,group,mask); + } +} + + +void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) +{ + BT_PROFILE("updateActions"); + + for ( int i=0;i<m_actions.size();i++) + { + m_actions[i]->updateAction( this, timeStep); + } +} + + +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BT_PROFILE("updateActivationState"); + + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body) + { + body->updateDeactivation(timeStep); + + if (body->wantsSleeping()) + { + if (body->isStaticOrKinematicObject()) + { + body->setActivationState(ISLAND_SLEEPING); + } else + { + if (body->getActivationState() == ACTIVE_TAG) + body->setActivationState( WANTS_DEACTIVATION ); + if (body->getActivationState() == ISLAND_SLEEPING) + { + body->setAngularVelocity(btVector3(0,0,0)); + body->setLinearVelocity(btVector3(0,0,0)); + } + + } + } else + { + if (body->getActivationState() != DISABLE_DEACTIVATION) + body->setActivationState( ACTIVE_TAG ); + } + } + } +} + +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) +{ + m_constraints.push_back(constraint); + //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) + btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB()); + + if (disableCollisionsBetweenLinkedBodies) + { + constraint->getRigidBodyA().addConstraintRef(constraint); + constraint->getRigidBodyB().addConstraintRef(constraint); + } +} + +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +{ + m_constraints.remove(constraint); + constraint->getRigidBodyA().removeConstraintRef(constraint); + constraint->getRigidBodyB().removeConstraintRef(constraint); +} + +void btDiscreteDynamicsWorld::addAction(btActionInterface* action) +{ + m_actions.push_back(action); +} + +void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) +{ + m_actions.remove(action); +} + + +void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) +{ + addAction(vehicle); +} + +void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) +{ + removeAction(vehicle); +} + +void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) +{ + addAction(character); +} + +void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) +{ + removeAction(character); +} + + + + +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + BT_PROFILE("solveConstraints"); + + m_sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;i<getNumConstraints();i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + +// btAssert(0); + + + + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); + + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback); + + m_solverIslandCallback->processConstraints(); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); +} + + +void btDiscreteDynamicsWorld::calculateSimulationIslands() +{ + BT_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + for (int i=0;i<this->m_predictiveManifolds.size();i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + if (constraint->isEnabled()) + { + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + + +} + + + + +class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback +{ +public: + + btCollisionObject* m_me; + btScalar m_allowedPenetration; + btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; + +public: + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + btCollisionWorld::ClosestConvexResultCallback(fromA,toA), + m_me(me), + m_allowedPenetration(0.0f), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + { + if (convexResult.m_hitCollisionObject == m_me) + return 1.0f; + + //ignore result if there is no contact response + if(!convexResult.m_hitCollisionObject->hasContactResponse()) + return 1.0f; + + btVector3 linVelA,linVelB; + linVelA = m_convexToWorld-m_convexFromWorld; + linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); + + btVector3 relativeVelocity = (linVelA-linVelB); + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + return 1.f; + + return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + //don't collide with itself + if (proxy0->m_clientObject == m_me) + return false; + + ///don't do CCD when the collision filters are not matching + if (!ClosestConvexResultCallback::needsCollision(proxy0)) + return false; + + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + + //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 + if (m_dispatcher->needsResponse(m_me,otherObj)) + { +#if 0 + ///don't do CCD when there are already contact points (touching contact/penetration) + btAlignedObjectArray<btPersistentManifold*> manifoldArray; + btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); + if (collisionPair) + { + if (collisionPair->m_algorithm) + { + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;j<manifoldArray.size();j++) + { + btPersistentManifold* manifold = manifoldArray[j]; + if (manifold->getNumContacts()>0) + return false; + } + } + } +#endif + return true; + } + + return false; + } + + +}; + +///internal debugging variable. this value shouldn't be too high +int gNumClampedCcdMotions=0; + + +void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep) +{ + btTransform predictedTrans; + for ( int i=0;i<numBodies;i++) + { + btRigidBody* body = bodies[i]; + body->setHitFraction(1.f); + + if (body->isActive() && (!body->isStaticOrKinematicObject())) + { + + body->predictIntegratedTransform(timeStep, predictedTrans); + + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + + if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + { + BT_PROFILE("predictive convexSweepTest"); + if (body->getCollisionShape()->isConvex()) + { + gNumClampedCcdMotions++; +#ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY + class StaticOnlyCallback : public btClosestNotMeConvexResultCallback + { + public: + + StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + if (!otherObj->isStaticOrKinematicObject()) + return false; + return btClosestNotMeConvexResultCallback::needsCollision(proxy0); + } + }; + + StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#else + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#endif + //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + btTransform modifiedPredictedTrans = predictedTrans; + modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); + + convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) + { + + btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; + btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); + + + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); + btMutexLock( &m_predictiveManifoldsMutex ); + m_predictiveManifolds.push_back(manifold); + btMutexUnlock( &m_predictiveManifoldsMutex ); + + btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; + btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; + + btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance); + + bool isPredictive = true; + int index = manifold->addManifoldPoint(newPoint, isPredictive); + btManifoldPoint& pt = manifold->getContactPoint(index); + pt.m_combinedRestitution = 0; + pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body,sweepResults.m_hitCollisionObject); + pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); + pt.m_positionWorldOnB = worldPointB; + + } + } + } + } + } +} + +void btDiscreteDynamicsWorld::releasePredictiveContacts() +{ + BT_PROFILE( "release predictive contact manifolds" ); + + for ( int i = 0; i < m_predictiveManifolds.size(); i++ ) + { + btPersistentManifold* manifold = m_predictiveManifolds[ i ]; + this->m_dispatcher1->releaseManifold( manifold ); + } + m_predictiveManifolds.clear(); +} + +void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +{ + BT_PROFILE("createPredictiveContacts"); + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + createPredictiveContactsInternal( &m_nonStaticRigidBodies[ 0 ], m_nonStaticRigidBodies.size(), timeStep ); + } +} + +void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ) +{ + btTransform predictedTrans; + for (int i=0;i<numBodies;i++) + { + btRigidBody* body = bodies[i]; + body->setHitFraction(1.f); + + if (body->isActive() && (!body->isStaticOrKinematicObject())) + { + + body->predictIntegratedTransform(timeStep, predictedTrans); + + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + + + + if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + { + BT_PROFILE("CCD motion clamping"); + if (body->getCollisionShape()->isConvex()) + { + gNumClampedCcdMotions++; +#ifdef USE_STATIC_ONLY + class StaticOnlyCallback : public btClosestNotMeConvexResultCallback + { + public: + + StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + if (!otherObj->isStaticOrKinematicObject()) + return false; + return btClosestNotMeConvexResultCallback::needsCollision(proxy0); + } + }; + + StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#else + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#endif + //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + btTransform modifiedPredictedTrans = predictedTrans; + modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); + + convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) + { + + //printf("clamped integration to hit fraction = %f\n",fraction); + body->setHitFraction(sweepResults.m_closestHitFraction); + body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->setHitFraction(0.f); + body->proceedToTransform( predictedTrans); + +#if 0 + btVector3 linVel = body->getLinearVelocity(); + + btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep; + btScalar maxSpeedSqr = maxSpeed*maxSpeed; + if (linVel.length2()>maxSpeedSqr) + { + linVel.normalize(); + linVel*= maxSpeed; + body->setLinearVelocity(linVel); + btScalar ms2 = body->getLinearVelocity().length2(); + body->predictIntegratedTransform(timeStep, predictedTrans); + + btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + btScalar smt = body->getCcdSquareMotionThreshold(); + printf("sm2=%f\n",sm2); + } +#else + + //don't apply the collision response right now, it will happen next frame + //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution. + //btScalar appliedImpulse = 0.f; + //btScalar depth = 0.f; + //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); + + +#endif + + continue; + } + } + } + + + body->proceedToTransform( predictedTrans); + + } + + } + +} + +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + BT_PROFILE("integrateTransforms"); + if (m_nonStaticRigidBodies.size() > 0) + { + integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } + + ///this should probably be switched on by default, but it is not well tested yet + if (m_applySpeculativeContactRestitution) + { + BT_PROFILE("apply speculative contact restitution"); + for (int i=0;i<m_predictiveManifolds.size();i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0()); + btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1()); + + for (int p=0;p<manifold->getNumContacts();p++) + { + const btManifoldPoint& pt = manifold->getContactPoint(p); + btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1); + + if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f) + //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f) + { + btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution; + + const btVector3& pos1 = pt.getPositionWorldOnA(); + const btVector3& pos2 = pt.getPositionWorldOnB(); + + btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); + btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); + + if (body0) + body0->applyImpulse(imp,rel_pos0); + if (body1) + body1->applyImpulse(-imp,rel_pos1); + } + } + } + } +} + + + + + +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + BT_PROFILE("predictUnconstraintMotion"); + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (!body->isStaticOrKinematicObject()) + { + //don't integrate/update velocities here, it happens in the constraint solver + + body->applyDamping(timeStep); + + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + } + } +} + + +void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) +{ + (void)timeStep; + +#ifndef BT_NO_PROFILE + CProfileManager::Reset(); +#endif //BT_NO_PROFILE + +} + + + + + + +void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) +{ + bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; + bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; + btScalar dbgDrawSize = constraint->getDbgDrawSize(); + if(dbgDrawSize <= btScalar(0.f)) + { + return; + } + + switch(constraint->getConstraintType()) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; + case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if(minAng == maxAng) + { + break; + } + bool drawSect = true; + if(!pHinge->hasLimit()) + { + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; + } + if(drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); + } + } + break; + case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8*4; + btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i=0; i<nSegments; i++) + { + fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments); + btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); + + if (i%(nSegments/8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); + + pPrev = pCur; + } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if(useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + + } + } + break; + case D6_SPRING_CONSTRAINT_TYPE: + case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; + ref[1] = -sz*axis[0] + cz*axis[1]; + ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + tr = p6DOF->getCalculatedTransformB(); + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if(minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); + } + else if(minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + } + } + break; + ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage + case D6_SPRING_2_CONSTRAINT_TYPE: + { + { + btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) + { + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; + ref[1] = -sz*axis[0] + cz*axis[1]; + ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + tr = p6DOF->getCalculatedTransformB(); + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if (minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false); + } + else if (minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0)); + } + } + break; + } + case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); + } + } + break; + default : + break; + } + return; +} + + + + + +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (m_ownsConstraintSolver) + { + btAlignedFree( m_constraintSolver); + } + m_ownsConstraintSolver = false; + m_constraintSolver = solver; + m_solverIslandCallback->m_solver = solver; +} + +btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} + + +int btDiscreteDynamicsWorld::getNumConstraints() const +{ + return int(m_constraints.size()); +} +btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) +{ + return m_constraints[index]; +} +const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const +{ + return m_constraints[index]; +} + + + +void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); + } + } + + for (i=0;i<m_constraints.size();i++) + { + btTypedConstraint* constraint = m_constraints[i]; + int size = constraint->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(size,1); + const char* structType = constraint->serialize(chunk->m_oldPtr,serializer); + serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint); + } +} + + + + +void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) +{ +#ifdef BT_USE_DOUBLE_PRECISION + int len = sizeof(btDynamicsWorldDoubleData); + btChunk* chunk = serializer->allocate(len,1); + btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; +#else//BT_USE_DOUBLE_PRECISION + int len = sizeof(btDynamicsWorldFloatData); + btChunk* chunk = serializer->allocate(len,1); + btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; +#endif//BT_USE_DOUBLE_PRECISION + + memset(worldInfo ,0x00,len); + + m_gravity.serialize(worldInfo->m_gravity); + worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; + worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; + worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; + worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; + + worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; + worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; + worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; + worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; + + worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; + worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; + worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; + worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; + + worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; + worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; + worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; + worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; + + worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; + worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; + worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; + worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; + + worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; + + // Fill padding with zeros to appease msan. + memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding)); + +#ifdef BT_USE_DOUBLE_PRECISION + const char* structType = "btDynamicsWorldDoubleData"; +#else//BT_USE_DOUBLE_PRECISION + const char* structType = "btDynamicsWorldFloatData"; +#endif//BT_USE_DOUBLE_PRECISION + serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo); +} + +void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeDynamicsWorldInfo(serializer); + + serializeCollisionObjects(serializer); + + serializeRigidBodies(serializer); + + serializer->finishSerialization(); +} + |