| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| |
| #include "BulletDynamics/Dynamics/btContinuousDynamicsWorld.h" |
| #include "LinearMath/btQuickprof.h" |
| |
| //collision detection |
| #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" |
| #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" |
| #include "BulletCollision/CollisionShapes/btCollisionShape.h" |
| #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" |
| |
| //rigidbody & constraints |
| #include "BulletDynamics/Dynamics/btRigidBody.h" |
| #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" |
| #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" |
| #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" |
| |
| |
| |
| #include <stdio.h> |
| |
| btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) |
| :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) |
| { |
| } |
| |
| btContinuousDynamicsWorld::~btContinuousDynamicsWorld() |
| { |
| } |
| |
| |
| void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) |
| { |
| |
| startProfiling(timeStep); |
| |
| if(0 != m_internalPreTickCallback) { |
| (*m_internalPreTickCallback)(this, timeStep); |
| } |
| |
| |
| ///update aabbs information |
| updateAabbs(); |
| //static int frame=0; |
| // printf("frame %d\n",frame++); |
| |
| ///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(); |
| calculateTimeOfImpacts(timeStep); |
| |
| btScalar toi = dispatchInfo.m_timeOfImpact; |
| // if (toi < 1.f) |
| // printf("toi = %f\n",toi); |
| if (toi < 0.f) |
| printf("toi = %f\n",toi); |
| |
| |
| ///integrate transforms |
| integrateTransforms(timeStep * toi); |
| |
| ///update vehicle simulation |
| updateActions(timeStep); |
| |
| updateActivationState( timeStep ); |
| |
| if(0 != m_internalTickCallback) { |
| (*m_internalTickCallback)(this, timeStep); |
| } |
| } |
| |
| void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) |
| { |
| ///these should be 'temporal' aabbs! |
| updateTemporalAabbs(timeStep); |
| |
| ///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually. |
| ///so we handle the case moving versus static properly, and we cheat for moving versus moving |
| btScalar toi = 1.f; |
| |
| |
| btDispatcherInfo& dispatchInfo = getDispatchInfo(); |
| dispatchInfo.m_timeStep = timeStep; |
| dispatchInfo.m_timeOfImpact = 1.f; |
| dispatchInfo.m_stepCount = 0; |
| dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; |
| |
| ///calculate time of impact for overlapping pairs |
| |
| |
| btDispatcher* dispatcher = getDispatcher(); |
| if (dispatcher) |
| dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); |
| |
| toi = dispatchInfo.m_timeOfImpact; |
| |
| dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE; |
| |
| } |
| |
| void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) |
| { |
| |
| btVector3 temporalAabbMin,temporalAabbMax; |
| |
| for ( int i=0;i<m_collisionObjects.size();i++) |
| { |
| btCollisionObject* colObj = m_collisionObjects[i]; |
| |
| btRigidBody* body = btRigidBody::upcast(colObj); |
| if (body) |
| { |
| body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax); |
| const btVector3& linvel = body->getLinearVelocity(); |
| |
| //make the AABB temporal |
| btScalar temporalAabbMaxx = temporalAabbMax.getX(); |
| btScalar temporalAabbMaxy = temporalAabbMax.getY(); |
| btScalar temporalAabbMaxz = temporalAabbMax.getZ(); |
| btScalar temporalAabbMinx = temporalAabbMin.getX(); |
| btScalar temporalAabbMiny = temporalAabbMin.getY(); |
| btScalar temporalAabbMinz = temporalAabbMin.getZ(); |
| |
| // add linear motion |
| btVector3 linMotion = linvel*timeStep; |
| |
| if (linMotion.x() > 0.f) |
| temporalAabbMaxx += linMotion.x(); |
| else |
| temporalAabbMinx += linMotion.x(); |
| if (linMotion.y() > 0.f) |
| temporalAabbMaxy += linMotion.y(); |
| else |
| temporalAabbMiny += linMotion.y(); |
| if (linMotion.z() > 0.f) |
| temporalAabbMaxz += linMotion.z(); |
| else |
| temporalAabbMinz += linMotion.z(); |
| |
| //add conservative angular motion |
| btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep; |
| btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); |
| temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); |
| temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); |
| |
| temporalAabbMin -= angularMotion3d; |
| temporalAabbMax += angularMotion3d; |
| |
| m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1); |
| } |
| } |
| |
| //update aabb (of all moved objects) |
| |
| m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); |
| |
| |
| |
| } |
| |
| |
| |