diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp new file mode 100644 index 000000000..d765b934c --- /dev/null +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -0,0 +1,284 @@ + +#include "btDiscreteDynamicsWorld.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld() +:btDynamicsWorld(new CollisionDispatcher(),new SimpleBroadphase()), +m_constraintSolver(new SequentialImpulseConstraintSolver) +{ + m_islandManager = new SimulationIslandManager(); +} + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache,ConstraintSolver* constraintSolver) +:btDynamicsWorld(dispatcher,pairCache), +m_constraintSolver(constraintSolver) +{ + m_islandManager = new SimulationIslandManager(); +} + + +btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() +{ + delete m_islandManager ; + + delete m_constraintSolver; + + //delete the dispatcher and paircache + delete m_dispatcher1; + m_dispatcher1 = 0; + delete m_pairCache; + m_pairCache = 0; +} + +void btDiscreteDynamicsWorld::stepSimulation(float timeStep) +{ + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection + PerformDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + ContactSolverInfo infoGlobal; + infoGlobal.m_timeStep = timeStep; + + ///solve non-contact constraints + solveNoncontactConstraints(infoGlobal); + + solveContactConstraints(infoGlobal); + + //CallbackTriggers(); + + ///integrate transforms + integrateTransforms(timeStep); + + updateActivationState( timeStep ); + + updateAabbs(); + +} + +void btDiscreteDynamicsWorld::updateActivationState(float timeStep) +{ + for (int i=0;im_internalOwner) + { + RigidBody* body = (RigidBody*)colObj->m_internalOwner; + + body->updateDeactivation(timeStep); + + if (body->wantsSleeping()) + { + if (body->GetActivationState() == ACTIVE_TAG) + body->SetActivationState( WANTS_DEACTIVATION ); + } else + { + if (body->GetActivationState() != DISABLE_DEACTIVATION) + body->SetActivationState( ACTIVE_TAG ); + } + } + } +} + +void btDiscreteDynamicsWorld::addConstraint(TypedConstraint* constraint) +{ + m_constraints.push_back(constraint); +} + +void btDiscreteDynamicsWorld::removeConstraint(TypedConstraint* constraint) +{ + std::vector::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint); + if (!(cit==m_constraints.end())) + { + m_constraints.erase(cit); + } +} + +void btDiscreteDynamicsWorld::solveContactConstraints(ContactSolverInfo& solverInfo) +{ + + struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback + { + + ContactSolverInfo& m_solverInfo; + ConstraintSolver* m_solver; + IDebugDraw* m_debugDrawer; + + InplaceSolverIslandCallback( + ContactSolverInfo& solverInfo, + ConstraintSolver* solver, + IDebugDraw* debugDrawer) + :m_solverInfo(solverInfo), + m_solver(solver), + m_debugDrawer(debugDrawer) + { + + } + + virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) + { + m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); + } + + }; + + IDebugDraw* debugDraw = 0; + InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, debugDraw); + + + /// solve all the contact points and contact friction + m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),GetCollisionWorld()->GetCollisionObjectArray(),&solverCallback); + + +} + + +void btDiscreteDynamicsWorld::solveNoncontactConstraints(ContactSolverInfo& solverInfo) +{ + #ifdef USE_QUICKPROF + Profiler::beginBlock("SolveConstraint"); +#endif //USE_QUICKPROF + + + + int i; + int numConstraints = m_constraints.size(); + + ///constraint preparation: building jacobians + for (i=0;i< numConstraints ; i++ ) + { + TypedConstraint* constraint = m_constraints[i]; + constraint->BuildJacobian(); + } + + //solve the regular non-contact constraints (point 2 point, hinge, generic d6) + for (int g=0;gSolveConstraint( solverInfo.m_timeStep ); + } + } + +#ifdef USE_QUICKPROF + Profiler::endBlock("SolveConstraint"); +#endif //USE_QUICKPROF + +} + +void btDiscreteDynamicsWorld::calculateSimulationIslands() +{ + +#ifdef USE_QUICKPROF + Profiler::beginBlock("IslandUnionFind"); +#endif //USE_QUICKPROF + + GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); + + { + int i; + int numConstraints = m_constraints.size(); + for (i=0;i< numConstraints ; i++ ) + { + TypedConstraint* constraint = m_constraints[i]; + + const RigidBody* colObj0 = &constraint->GetRigidBodyA(); + const RigidBody* colObj1 = &constraint->GetRigidBodyB(); + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + if (colObj0->IsActive() || colObj1->IsActive()) + { + + GetSimulationIslandManager()->GetUnionFind().unite((colObj0)->m_islandTag1, + (colObj1)->m_islandTag1); + } + } + } + } + + //Store the island id in each body + GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld()); + +#ifdef USE_QUICKPROF + Profiler::endBlock("IslandUnionFind"); +#endif //USE_QUICKPROF + +} + +void btDiscreteDynamicsWorld::updateAabbs() +{ + SimdTransform predictedTrans; + for (int i=0;im_internalOwner) + { + RigidBody* body = (RigidBody*)colObj->m_internalOwner; + if (body->IsActive() && (!body->IsStatic())) + { + SimdPoint3 minAabb,maxAabb; + colObj->m_collisionShape->GetAabb(colObj->m_worldTransform, minAabb,maxAabb); + SimpleBroadphase* bp = (SimpleBroadphase*)m_pairCache; + bp->SetAabb(body->m_broadphaseHandle,minAabb,maxAabb); + } + } + } +} + +void btDiscreteDynamicsWorld::integrateTransforms(float timeStep) +{ + SimdTransform predictedTrans; + for (int i=0;im_internalOwner) + { + RigidBody* body = (RigidBody*)colObj->m_internalOwner; + if (body->IsActive() && (!body->IsStatic())) + { + body->predictIntegratedTransform(timeStep, predictedTrans); + body->proceedToTransform( predictedTrans); + } + } + } +} + + + +void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep) +{ + for (int i=0;im_internalOwner) + { + RigidBody* body = (RigidBody*)colObj->m_internalOwner; + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); + if (body->IsActive() && (!body->IsStatic())) + { + body->applyForces( timeStep); + body->integrateVelocities( timeStep); + body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + + } + } + } +} \ No newline at end of file diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h new file mode 100644 index 000000000..7c4dd2485 --- /dev/null +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -0,0 +1,89 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DISCRETE_DYNAMICS_WORLD_H +#define BT_DISCRETE_DYNAMICS_WORLD_H + +#include "btDynamicsWorld.h" + +class Dispatcher; +class OverlappingPairCache; +class ConstraintSolver; +class SimulationIslandManager; +class TypedConstraint; +struct ContactSolverInfo; + +#include + +///btDiscreteDynamicsWorld provides discrete rigid body simulation +///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +class btDiscreteDynamicsWorld : public btDynamicsWorld +{ +protected: + + ConstraintSolver* m_constraintSolver; + + SimulationIslandManager* m_islandManager; + + std::vector m_constraints; + + void predictUnconstraintMotion(float timeStep); + + void integrateTransforms(float timeStep); + + void updateAabbs(); + + void calculateSimulationIslands(); + + void solveNoncontactConstraints(ContactSolverInfo& solverInfo); + + void solveContactConstraints(ContactSolverInfo& solverInfo); + + void updateActivationState(float timeStep); + + +public: + + + btDiscreteDynamicsWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache,ConstraintSolver* constraintSolver); + + btDiscreteDynamicsWorld(); + + virtual ~btDiscreteDynamicsWorld(); + + virtual void stepSimulation( float timeStep); + + void addConstraint(TypedConstraint* constraint); + + void removeConstraint(TypedConstraint* constraint); + + SimulationIslandManager* GetSimulationIslandManager() + { + return m_islandManager; + } + + const SimulationIslandManager* GetSimulationIslandManager() const + { + return m_islandManager; + } + + CollisionWorld* GetCollisionWorld() + { + return this; + } + +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H \ No newline at end of file diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h new file mode 100644 index 000000000..7621b84f3 --- /dev/null +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DYNAMICS_WORLD_H +#define BT_DYNAMICS_WORLD_H + +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +class TypedConstraint; + +///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous +class btDynamicsWorld : public CollisionWorld +{ + public: + + btDynamicsWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache) + :CollisionWorld(dispatcher,pairCache) + { + + } + virtual ~btDynamicsWorld() + { + } + + ///stepSimulation proceeds the simulation over timeStep units + virtual void stepSimulation( float timeStep) = 0; + + + virtual void addConstraint(TypedConstraint* constraint) {}; + + virtual void removeConstraint(TypedConstraint* constraint) {}; + +}; + +#endif //BT_DYNAMICS_WORLD_H \ No newline at end of file diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 1e26130f6..b4ace618a 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -9,12 +9,20 @@ btSimpleDynamicsWorld::btSimpleDynamicsWorld() -:CollisionWorld(new CollisionDispatcher(),new SimpleBroadphase()), +:btDynamicsWorld(new CollisionDispatcher(),new SimpleBroadphase()), m_constraintSolver(new SequentialImpulseConstraintSolver) { } +btSimpleDynamicsWorld::btSimpleDynamicsWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache,ConstraintSolver* constraintSolver) +:btDynamicsWorld(dispatcher,pairCache), +m_constraintSolver(constraintSolver) +{ + +} + + btSimpleDynamicsWorld::~btSimpleDynamicsWorld() { delete m_constraintSolver; @@ -49,6 +57,8 @@ void btSimpleDynamicsWorld::stepSimulation(float timeStep) } + + void btSimpleDynamicsWorld::updateAabbs() { SimdTransform predictedTrans; diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 72590c153..07b58701d 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -16,19 +16,22 @@ subject to the following restrictions: #ifndef BT_SIMPLE_DYNAMICS_WORLD_H #define BT_SIMPLE_DYNAMICS_WORLD_H -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -class SequentialImpulseConstraintSolver; +#include "btDynamicsWorld.h" + +class Dispatcher; +class OverlappingPairCache; +class ConstraintSolver; ///btSimpleDynamicsWorld demonstrates very basic usage of Bullet rigid body dynamics ///It can be used for basic simulations, and as a starting point for porting Bullet ///btSimpleDynamicsWorld lacks object deactivation, island management and other concepts. ///For more complicated simulations, btDiscreteDynamicsWorld and btContinuousDynamicsWorld are recommended ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -class btSimpleDynamicsWorld : public CollisionWorld +class btSimpleDynamicsWorld : public btDynamicsWorld { protected: - SequentialImpulseConstraintSolver* m_constraintSolver; + ConstraintSolver* m_constraintSolver; void predictUnconstraintMotion(float timeStep); @@ -38,8 +41,11 @@ protected: public: + + btSimpleDynamicsWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache,ConstraintSolver* constraintSolver); + btSimpleDynamicsWorld(); - + virtual ~btSimpleDynamicsWorld(); virtual void stepSimulation( float timeStep);