added replacement for CcdPhysicsEnvironment
This commit is contained in:
ejcoumans
284
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
Normal file
284
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
Normal file
@@ -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 <algorithm>
|
||||
|
||||
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;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
CollisionObject* colObj = m_collisionObjects[i];
|
||||
if (colObj->m_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<TypedConstraint*>::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;g<solverInfo.m_numIterations;g++)
|
||||
{
|
||||
//
|
||||
// constraint solving
|
||||
//
|
||||
for (i=0;i< numConstraints ; i++ )
|
||||
{
|
||||
TypedConstraint* constraint = m_constraints[i];
|
||||
constraint->SolveConstraint( 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;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
CollisionObject* colObj = m_collisionObjects[i];
|
||||
if (colObj->m_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;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
CollisionObject* colObj = m_collisionObjects[i];
|
||||
if (colObj->m_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;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
CollisionObject* colObj = m_collisionObjects[i];
|
||||
if (colObj->m_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);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user