| /* | |
| 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 "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" | |
| //collision detection | |
| #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" | |
| #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.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/btSliderConstraint.h" | |
| //for debug rendering | |
| #include "BulletCollision/CollisionShapes/btBoxShape.h" | |
| #include "BulletCollision/CollisionShapes/btCapsuleShape.h" | |
| #include "BulletCollision/CollisionShapes/btCompoundShape.h" | |
| #include "BulletCollision/CollisionShapes/btConeShape.h" | |
| #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" | |
| #include "BulletCollision/CollisionShapes/btCylinderShape.h" | |
| #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" | |
| #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" | |
| #include "BulletCollision/CollisionShapes/btSphereShape.h" | |
| #include "BulletCollision/CollisionShapes/btTriangleCallback.h" | |
| #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" | |
| #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" | |
| #include "LinearMath/btIDebugDraw.h" | |
| #include "BulletDynamics/Dynamics/btActionInterface.h" | |
| #include "LinearMath/btQuickprof.h" | |
| #include "LinearMath/btMotionState.h" | |
| btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) | |
| :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), | |
| m_constraintSolver(constraintSolver), | |
| m_gravity(0,-10,0), | |
| m_localTime(btScalar(1.)/btScalar(60.)), | |
| m_synchronizeAllMotionStates(false), | |
| m_profileTimings(0) | |
| { | |
| 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; | |
| } | |
| btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() | |
| { | |
| //only delete it when we created it | |
| if (m_ownsIslandManager) | |
| { | |
| m_islandManager->~btSimulationIslandManager(); | |
| btAlignedFree( m_islandManager); | |
| } | |
| 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"); | |
| if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) | |
| { | |
| int numManifolds = getDispatcher()->getNumManifolds(); | |
| btVector3 color(0,0,0); | |
| for (int i=0;i<numManifolds;i++) | |
| { | |
| btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); | |
| //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); | |
| //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); | |
| int numContacts = contactManifold->getNumContacts(); | |
| for (int j=0;j<numContacts;j++) | |
| { | |
| btManifoldPoint& cp = contactManifold->getContactPoint(j); | |
| getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); | |
| } | |
| } | |
| } | |
| 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)) | |
| { | |
| int i; | |
| for ( i=0;i<m_collisionObjects.size();i++) | |
| { | |
| btCollisionObject* colObj = m_collisionObjects[i]; | |
| if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) | |
| { | |
| btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); | |
| switch(colObj->getActivationState()) | |
| { | |
| case ACTIVE_TAG: | |
| color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; | |
| case ISLAND_SLEEPING: | |
| color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; | |
| case WANTS_DEACTIVATION: | |
| color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; | |
| case DISABLE_DEACTIVATION: | |
| color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; | |
| case DISABLE_SIMULATION: | |
| color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; | |
| default: | |
| { | |
| color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); | |
| } | |
| }; | |
| debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); | |
| } | |
| if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) | |
| { | |
| btVector3 minAabb,maxAabb; | |
| btVector3 colorvec(1,0,0); | |
| colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); | |
| m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); | |
| } | |
| } | |
| if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) | |
| { | |
| for (i=0;i<m_actions.size();i++) | |
| { | |
| m_actions[i]->debugDraw(m_debugDrawer); | |
| } | |
| } | |
| } | |
| } | |
| 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_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); | |
| BT_PROFILE("stepSimulation"); | |
| int numSimulationSubSteps = 0; | |
| if (maxSubSteps) | |
| { | |
| //fixed timestep with interpolation | |
| m_localTime += timeStep; | |
| if (m_localTime >= fixedTimeStep) | |
| { | |
| numSimulationSubSteps = int( m_localTime / fixedTimeStep); | |
| m_localTime -= numSimulationSubSteps * fixedTimeStep; | |
| } | |
| } else | |
| { | |
| //variable timestep | |
| fixedTimeStep = timeStep; | |
| m_localTime = timeStep; | |
| 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) | |
| { | |
| saveKinematicState(fixedTimeStep); | |
| applyGravity(); | |
| //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt | |
| int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; | |
| 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(); | |
| ///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->setGravity(gravity); | |
| } | |
| } | |
| } | |
| btVector3 btDiscreteDynamicsWorld::getGravity () const | |
| { | |
| return m_gravity; | |
| } | |
| void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short 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->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()); | |
| short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); | |
| short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); | |
| addCollisionObject(body,collisionFilterGroup,collisionFilterMask); | |
| } | |
| } | |
| void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) | |
| { | |
| if (!body->isStaticOrKinematicObject()) | |
| { | |
| 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); | |
| 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); | |
| } | |
| 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 ) | |
| { | |
| int rIslandId0,lIslandId0; | |
| rIslandId0 = btGetConstraintIslandId(rhs); | |
| lIslandId0 = btGetConstraintIslandId(lhs); | |
| return lIslandId0 < rIslandId0; | |
| } | |
| }; | |
| void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) | |
| { | |
| BT_PROFILE("solveConstraints"); | |
| struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback | |
| { | |
| btContactSolverInfo& m_solverInfo; | |
| btConstraintSolver* m_solver; | |
| btTypedConstraint** m_sortedConstraints; | |
| int m_numConstraints; | |
| btIDebugDraw* m_debugDrawer; | |
| btStackAlloc* m_stackAlloc; | |
| btDispatcher* m_dispatcher; | |
| InplaceSolverIslandCallback( | |
| btContactSolverInfo& solverInfo, | |
| btConstraintSolver* solver, | |
| btTypedConstraint** sortedConstraints, | |
| int numConstraints, | |
| btIDebugDraw* debugDrawer, | |
| btStackAlloc* stackAlloc, | |
| btDispatcher* dispatcher) | |
| :m_solverInfo(solverInfo), | |
| m_solver(solver), | |
| m_sortedConstraints(sortedConstraints), | |
| m_numConstraints(numConstraints), | |
| m_debugDrawer(debugDrawer), | |
| m_stackAlloc(stackAlloc), | |
| m_dispatcher(dispatcher) | |
| { | |
| } | |
| InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) | |
| { | |
| btAssert(0); | |
| (void)other; | |
| return *this; | |
| } | |
| virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) | |
| { | |
| if (islandId<0) | |
| { | |
| if (numManifolds + m_numConstraints) | |
| { | |
| ///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_stackAlloc,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++; | |
| } | |
| } | |
| ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive | |
| if (numManifolds + numCurConstraints) | |
| { | |
| m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); | |
| } | |
| } | |
| } | |
| }; | |
| //sorted version of all btTypedConstraint, based on islandId | |
| btAlignedObjectArray<btTypedConstraint*> sortedConstraints; | |
| sortedConstraints.resize( m_constraints.size()); | |
| int i; | |
| for (i=0;i<getNumConstraints();i++) | |
| { | |
| sortedConstraints[i] = m_constraints[i]; | |
| } | |
| // btAssert(0); | |
| sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); | |
| btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; | |
| InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1); | |
| m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); | |
| /// solve all the constraints for this island | |
| m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); | |
| m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); | |
| } | |
| void btDiscreteDynamicsWorld::calculateSimulationIslands() | |
| { | |
| BT_PROFILE("calculateSimulationIslands"); | |
| getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); | |
| { | |
| int i; | |
| int numConstraints = int(m_constraints.size()); | |
| for (i=0;i< numConstraints ; i++ ) | |
| { | |
| btTypedConstraint* constraint = m_constraints[i]; | |
| const btRigidBody* colObj0 = &constraint->getRigidBodyA(); | |
| const btRigidBody* colObj1 = &constraint->getRigidBodyB(); | |
| if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && | |
| ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) | |
| { | |
| if (colObj0->isActive() || colObj1->isActive()) | |
| { | |
| getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), | |
| (colObj1)->getIslandTag()); | |
| } | |
| } | |
| } | |
| } | |
| //Store the island id in each body | |
| getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); | |
| } | |
| #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" | |
| class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback | |
| { | |
| 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_allowedPenetration(0.0f), | |
| m_me(me), | |
| 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)) | |
| { | |
| ///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; | |
| } | |
| } | |
| } | |
| } | |
| return true; | |
| } | |
| }; | |
| ///internal debugging variable. this value shouldn't be too high | |
| int gNumClampedCcdMotions=0; | |
| //#include "stdio.h" | |
| void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) | |
| { | |
| BT_PROFILE("integrateTransforms"); | |
| btTransform predictedTrans; | |
| for ( int i=0;i<m_nonStaticRigidBodies.size();i++) | |
| { | |
| btRigidBody* body = m_nonStaticRigidBodies[i]; | |
| body->setHitFraction(1.f); | |
| if (body->isActive() && (!body->isStaticOrKinematicObject())) | |
| { | |
| body->predictIntegratedTransform(timeStep, predictedTrans); | |
| btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); | |
| if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) | |
| { | |
| BT_PROFILE("CCD motion clamping"); | |
| if (body->getCollisionShape()->isConvex()) | |
| { | |
| gNumClampedCcdMotions++; | |
| btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); | |
| //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); | |
| btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); | |
| sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; | |
| sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; | |
| convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); | |
| if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) | |
| { | |
| body->setHitFraction(sweepResults.m_closestHitFraction); | |
| body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); | |
| body->setHitFraction(0.f); | |
| // printf("clamped integration to hit fraction = %f\n",fraction); | |
| } | |
| } | |
| } | |
| body->proceedToTransform( predictedTrans); | |
| } | |
| } | |
| } | |
| 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()) | |
| { | |
| body->integrateVelocities( timeStep); | |
| //damping | |
| 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 | |
| } | |
| class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback | |
| { | |
| btIDebugDraw* m_debugDrawer; | |
| btVector3 m_color; | |
| btTransform m_worldTrans; | |
| public: | |
| DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : | |
| m_debugDrawer(debugDrawer), | |
| m_color(color), | |
| m_worldTrans(worldTrans) | |
| { | |
| } | |
| virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) | |
| { | |
| processTriangle(triangle,partId,triangleIndex); | |
| } | |
| virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) | |
| { | |
| (void)partId; | |
| (void)triangleIndex; | |
| btVector3 wv0,wv1,wv2; | |
| wv0 = m_worldTrans*triangle[0]; | |
| wv1 = m_worldTrans*triangle[1]; | |
| wv2 = m_worldTrans*triangle[2]; | |
| m_debugDrawer->drawLine(wv0,wv1,m_color); | |
| m_debugDrawer->drawLine(wv1,wv2,m_color); | |
| m_debugDrawer->drawLine(wv2,wv0,m_color); | |
| } | |
| }; | |
| void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color) | |
| { | |
| btVector3 start = transform.getOrigin(); | |
| const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); | |
| const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); | |
| const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); | |
| // XY | |
| getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color); | |
| getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color); | |
| getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color); | |
| getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color); | |
| // XZ | |
| getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color); | |
| getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color); | |
| getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color); | |
| getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color); | |
| // YZ | |
| getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color); | |
| getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color); | |
| getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color); | |
| getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color); | |
| } | |
| void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) | |
| { | |
| // Draw a small simplex at the center of the object | |
| { | |
| btVector3 start = worldTransform.getOrigin(); | |
| getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); | |
| getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); | |
| getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); | |
| } | |
| if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) | |
| { | |
| const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); | |
| for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) | |
| { | |
| btTransform childTrans = compoundShape->getChildTransform(i); | |
| const btCollisionShape* colShape = compoundShape->getChildShape(i); | |
| debugDrawObject(worldTransform*childTrans,colShape,color); | |
| } | |
| } else | |
| { | |
| switch (shape->getShapeType()) | |
| { | |
| case SPHERE_SHAPE_PROXYTYPE: | |
| { | |
| const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); | |
| btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin | |
| debugDrawSphere(radius, worldTransform, color); | |
| break; | |
| } | |
| case MULTI_SPHERE_SHAPE_PROXYTYPE: | |
| { | |
| const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); | |
| btTransform childTransform; | |
| childTransform.setIdentity(); | |
| for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) | |
| { | |
| childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); | |
| debugDrawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); | |
| } | |
| break; | |
| } | |
| case CAPSULE_SHAPE_PROXYTYPE: | |
| { | |
| const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); | |
| btScalar radius = capsuleShape->getRadius(); | |
| btScalar halfHeight = capsuleShape->getHalfHeight(); | |
| int upAxis = capsuleShape->getUpAxis(); | |
| btVector3 capStart(0.f,0.f,0.f); | |
| capStart[upAxis] = -halfHeight; | |
| btVector3 capEnd(0.f,0.f,0.f); | |
| capEnd[upAxis] = halfHeight; | |
| // Draw the ends | |
| { | |
| btTransform childTransform = worldTransform; | |
| childTransform.getOrigin() = worldTransform * capStart; | |
| debugDrawSphere(radius, childTransform, color); | |
| } | |
| { | |
| btTransform childTransform = worldTransform; | |
| childTransform.getOrigin() = worldTransform * capEnd; | |
| debugDrawSphere(radius, childTransform, color); | |
| } | |
| // Draw some additional lines | |
| btVector3 start = worldTransform.getOrigin(); | |
| capStart[(upAxis+1)%3] = radius; | |
| capEnd[(upAxis+1)%3] = radius; | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); | |
| capStart[(upAxis+1)%3] = -radius; | |
| capEnd[(upAxis+1)%3] = -radius; | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); | |
| capStart[(upAxis+1)%3] = 0.f; | |
| capEnd[(upAxis+1)%3] = 0.f; | |
| capStart[(upAxis+2)%3] = radius; | |
| capEnd[(upAxis+2)%3] = radius; | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); | |
| capStart[(upAxis+2)%3] = -radius; | |
| capEnd[(upAxis+2)%3] = -radius; | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); | |
| break; | |
| } | |
| case CONE_SHAPE_PROXYTYPE: | |
| { | |
| const btConeShape* coneShape = static_cast<const btConeShape*>(shape); | |
| btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); | |
| btScalar height = coneShape->getHeight();//+coneShape->getMargin(); | |
| btVector3 start = worldTransform.getOrigin(); | |
| int upAxis= coneShape->getConeUpIndex(); | |
| btVector3 offsetHeight(0,0,0); | |
| offsetHeight[upAxis] = height * btScalar(0.5); | |
| btVector3 offsetRadius(0,0,0); | |
| offsetRadius[(upAxis+1)%3] = radius; | |
| btVector3 offset2Radius(0,0,0); | |
| offset2Radius[(upAxis+2)%3] = radius; | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); | |
| break; | |
| } | |
| case CYLINDER_SHAPE_PROXYTYPE: | |
| { | |
| const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); | |
| int upAxis = cylinder->getUpAxis(); | |
| btScalar radius = cylinder->getRadius(); | |
| btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; | |
| btVector3 start = worldTransform.getOrigin(); | |
| btVector3 offsetHeight(0,0,0); | |
| offsetHeight[upAxis] = halfHeight; | |
| btVector3 offsetRadius(0,0,0); | |
| offsetRadius[(upAxis+1)%3] = radius; | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); | |
| getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); | |
| break; | |
| } | |
| case STATIC_PLANE_PROXYTYPE: | |
| { | |
| const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); | |
| btScalar planeConst = staticPlaneShape->getPlaneConstant(); | |
| const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); | |
| btVector3 planeOrigin = planeNormal * planeConst; | |
| btVector3 vec0,vec1; | |
| btPlaneSpace1(planeNormal,vec0,vec1); | |
| btScalar vecLen = 100.f; | |
| btVector3 pt0 = planeOrigin + vec0*vecLen; | |
| btVector3 pt1 = planeOrigin - vec0*vecLen; | |
| btVector3 pt2 = planeOrigin + vec1*vecLen; | |
| btVector3 pt3 = planeOrigin - vec1*vecLen; | |
| getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); | |
| getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); | |
| break; | |
| } | |
| default: | |
| { | |
| if (shape->isConcave()) | |
| { | |
| btConcaveShape* concaveMesh = (btConcaveShape*) shape; | |
| ///@todo pass camera, for some culling? no -> we are not a graphics lib | |
| btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); | |
| btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); | |
| DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); | |
| concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); | |
| } | |
| if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) | |
| { | |
| btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; | |
| //todo: pass camera for some culling | |
| btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); | |
| btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); | |
| //DebugDrawcallback drawCallback; | |
| DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); | |
| convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); | |
| } | |
| /// for polyhedral shapes | |
| if (shape->isPolyhedral()) | |
| { | |
| btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; | |
| int i; | |
| for (i=0;i<polyshape->getNumEdges();i++) | |
| { | |
| btVector3 a,b; | |
| polyshape->getEdge(i,a,b); | |
| btVector3 wa = worldTransform * a; | |
| btVector3 wb = worldTransform * b; | |
| getDebugDrawer()->drawLine(wa,wb,color); | |
| } | |
| } | |
| } | |
| } | |
| } | |
| } | |
| 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(minAng > maxAng) | |
| { | |
| 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_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; | |
| 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->getCalculatedTransformA(); | |
| 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; | |
| } | |
| 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]; | |
| } | |