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;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
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++;
}
}
if (m_solverInfo.m_minimumSolverBatchSize<=1)
{
///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);
}
} 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()
{
if (m_manifolds.size() + m_constraints.size()>0)
{
m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
}
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
}
};
//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);
solverCallback.processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
}
/// Perform an integration step of duration 'timeStep'.
bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
//printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
if (SimdFuzzyZero(timeStep))
return true;
if (m_debugDrawer)
{
gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
}
#ifdef USE_QUICKPROF
Profiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF
//this is needed because scaling is not known in advance, and scaling has to propagate to the shape
if (!m_scalingPropagated)
{
SyncMotionStates(timeStep);
m_scalingPropagated = true;
}
#ifdef USE_QUICKPROF
Profiler::endBlock("SyncMotionStates");
Profiler::beginBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
{
// std::vector<CcdPhysicsController*>::iterator i;
int k;
for (k=0;k<GetNumControllers();k++)
{
CcdPhysicsController* ctrl = m_controllers[k];
// SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
if (body->IsActive())
{
if (!body->IsStatic())
{
body->applyForces( timeStep);
body->integrateVelocities( timeStep);
body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
}
}
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
OverlappingPairCache* scene = m_collisionWorld->GetPairCache();
//
// collision detection (?)
//
#ifdef USE_QUICKPROF
Profiler::beginBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF
int numsubstep = m_numIterations;
DispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
dispatchInfo.m_debugDraw = this->m_debugDrawer;
scene->RefreshOverlappingPairs();
GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
#ifdef USE_QUICKPROF
Profiler::endBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF
int numRigidBodies = m_controllers.size();
m_islandManager->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())
{
m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
m_islandManager->StoreIslandActivationState(GetCollisionWorld());
//contacts
#ifdef USE_QUICKPROF
Profiler::beginBlock("SolveConstraint");
#endif //USE_QUICKPROF
//solve the regular constraints (point 2 point, hinge, etc)
for (int g=0;g<numsubstep;g++)
{
//
// constraint solving
//
int i;
int numConstraints = m_constraints.size();
//point to point constraints
for (i=0;i< numConstraints ; i++ )
{
TypedConstraint* constraint = m_constraints[i];
constraint->BuildJacobian();
constraint->SolveConstraint( timeStep );
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("SolveConstraint");
#endif //USE_QUICKPROF
//solve the vehicles
#ifdef NEW_BULLET_VEHICLE_SUPPORT
//vehicles
int numVehicles = m_wrapperVehicles.size();
for (int i=0;i<numVehicles;i++)
{
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
vehicle->UpdateVehicle( timeStep);
}
#endif //NEW_BULLET_VEHICLE_SUPPORT
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);
}
};
m_solverInfo.m_friction = 0.9f;
m_solverInfo.m_numIterations = m_numIterations;
m_solverInfo.m_timeStep = timeStep;
m_solverInfo.m_restitution = 0.f;//m_restitution;
InplaceSolverIslandCallback solverCallback(
m_solverInfo,
m_solver,
m_debugDrawer);
#ifdef USE_QUICKPROF
Profiler::beginBlock("BuildAndProcessIslands");
#endif //USE_QUICKPROF
/// solve all the contact points and contact friction
m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
#ifdef USE_QUICKPROF
Profiler::endBlock("BuildAndProcessIslands");
Profiler::beginBlock("CallbackTriggers");
#endif //USE_QUICKPROF
CallbackTriggers();
#ifdef USE_QUICKPROF
Profiler::endBlock("CallbackTriggers");
Profiler::beginBlock("proceedToTransform");
#endif //USE_QUICKPROF
{
{
UpdateAabbs(timeStep);
float toi = 1.f;
if (m_ccdMode == 3)
{
DispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
//pairCache->RefreshOverlappingPairs();//??
GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
toi = dispatchInfo.m_timeOfImpact;
}
//
// integrating solution
//
{
std::vector<CcdPhysicsController*>::iterator i;
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = *i;
SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
if (body->IsActive())
{
if (!body->IsStatic())
{
body->predictIntegratedTransform(timeStep* toi, predictedTrans);
body->proceedToTransform( predictedTrans);
}
}
}
}
//
// disable sleeping physics objects
//
std::vector<CcdPhysicsController*> m_sleepingControllers;
std::vector<CcdPhysicsController*>::iterator i;
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
ctrl->UpdateDeactivation(timeStep);
if (ctrl->wantsSleeping())
{
if (body->GetActivationState() == ACTIVE_TAG)
body->SetActivationState( WANTS_DEACTIVATION );
} else
{
if (body->GetActivationState() != DISABLE_DEACTIVATION)
body->SetActivationState( ACTIVE_TAG );
}
if (useIslands)
{
if (body->GetActivationState() == ISLAND_SLEEPING)
{
m_sleepingControllers.push_back(ctrl);
}
} else
{
if (ctrl->wantsSleeping())
{
m_sleepingControllers.push_back(ctrl);
}
}
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("proceedToTransform");
Profiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF
SyncMotionStates(timeStep);
#ifdef USE_QUICKPROF
Profiler::endBlock("SyncMotionStates");
Profiler::endProfilingCycle();
#endif //USE_QUICKPROF
#ifdef NEW_BULLET_VEHICLE_SUPPORT
//sync wheels for vehicles
int numVehicles = m_wrapperVehicles.size();
for (int i=0;i<numVehicles;i++)
{
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
wrapperVehicle->SyncWheels();
}
#endif //NEW_BULLET_VEHICLE_SUPPORT
}
return true;
}
