void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
// btAssert(0);
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
m_solverIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
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());
}
void btDiscreteDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
{
//merge islands based on speculative contact manifolds too
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
if (colObj0->isActive() || colObj1->isActive())
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
(colObj1)->getIslandTag());
}
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
if (constraint->isEnabled())
{
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());
}
void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
// -------------------------------------------------------------------------
Object::~Object()
{
if (mRootNode)
{
showDebugShape(false);
mShapeNode->detachObject (this);
mRootNode->removeAndDestroyChild (mShapeNode->getName ());
//mRootNode->getParentSceneNode ()->removeAndDestroyChild (mRootNode->getName ());
}
getBulletCollisionWorld()->removeCollisionObject( mObject );
getCollisionWorld()->removeObject(this);
delete mObject;
//delete mShape;
delete mState;
delete mDebugShape;
}
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);
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
{
if(!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
}
else
{
//
int numDofs = bod->getNumDofs() + 6;
int numPosVars = bod->getNumPosVars() + 7;
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
//convenience
btScalar *pMem = &scratch_r2[0];
btScalar *scratch_q0 = pMem; pMem += numPosVars;
btScalar *scratch_qx = pMem; pMem += numPosVars;
btScalar *scratch_qd0 = pMem; pMem += numDofs;
btScalar *scratch_qd1 = pMem; pMem += numDofs;
btScalar *scratch_qd2 = pMem; pMem += numDofs;
btScalar *scratch_qd3 = pMem; pMem += numDofs;
btScalar *scratch_qdd0 = pMem; pMem += numDofs;
btScalar *scratch_qdd1 = pMem; pMem += numDofs;
btScalar *scratch_qdd2 = pMem; pMem += numDofs;
btScalar *scratch_qdd3 = pMem; pMem += numDofs;
btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
/////
//copy q0 to scratch_q0 and qd0 to scratch_qd0
scratch_q0[0] = bod->getWorldToBaseRot().x();
scratch_q0[1] = bod->getWorldToBaseRot().y();
scratch_q0[2] = bod->getWorldToBaseRot().z();
scratch_q0[3] = bod->getWorldToBaseRot().w();
scratch_q0[4] = bod->getBasePos().x();
scratch_q0[5] = bod->getBasePos().y();
scratch_q0[6] = bod->getBasePos().z();
//
for(int link = 0; link < bod->getNumLinks(); ++link)
{
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
}
//
for(int dof = 0; dof < numDofs; ++dof)
scratch_qd0[dof] = bod->getVelocityVector()[dof];
////
struct
{
btMultiBody *bod;
btScalar *scratch_qx, *scratch_q0;
void operator()()
{
for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
scratch_qx[dof] = scratch_q0[dof];
}
} pResetQx = {bod, scratch_qx, scratch_q0};
//
struct
{
void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
{
for(int i = 0; i < size; ++i)
pVal[i] = pCurVal[i] + dt * pDer[i];
}
} pEulerIntegrate;
//
struct
{
void operator()(btMultiBody *pBody, const btScalar *pData)
{
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
pVel[i] = pData[i];
}
} pCopyToVelocityVector;
//
struct
{
void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
{
for(int i = 0; i < size; ++i)
pDst[i] = pSrc[start + i];
}
} pCopy;
//
btScalar h = solverInfo.m_timeStep;
#define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
//calc qd1 = qd0 + h/2 * qdd0
pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
//calc qd2 = qd0 + h/2 * qdd1
pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
//calc qd3 = qd0 + h * qdd2
pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
for(int i = 0; i < numDofs; ++i)
{
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
//delta_q[i] = h*scratch_qd0[i];
//delta_qd[i] = h*scratch_qdd0[i];
}
//
pCopyToVelocityVector(bod, scratch_qd0);
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
//
if(!doNotUpdatePos)
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
for(int i = 0; i < numDofs; ++i)
pRealBuf[i] = delta_q[i];
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->setPosUpdated(true);
}
//ugly hack which resets the cached data to t0 (needed for constraint solver)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}//if (!isSleeping)
}
}
clearMultiBodyConstraintForces();
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
}
}
}
}
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
{
//merge islands based on speculative contact manifolds too
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
if (constraint->isEnabled())
{
const btRigidBody* colObj0 = &constraint->getRigidBodyA();
const btRigidBody* colObj1 = &constraint->getRigidBodyB();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
}
//merge islands linked by Featherstone link colliders
for (int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
{
btMultiBodyLinkCollider* prev = body->getBaseCollider();
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
((prev) && (!(prev)->isStaticOrKinematicObject())))
{
int tagPrev = prev->getIslandTag();
int tagCur = cur->getIslandTag();
getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
}
if (cur && !cur->isStaticOrKinematicObject())
prev = cur;
}
}
}
//merge islands linked by multibody constraints
{
for (int i=0;i<this->m_multiBodyConstraints.size();i++)
{
btMultiBodyConstraint* c = m_multiBodyConstraints[i];
int tagA = c->getIslandIdA();
int tagB = c->getIslandIdB();
if (tagA>=0 && tagB>=0)
getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
}
}
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
{
BT_PROFILE("btMultiBody addForce and stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
scratch_r.resize(bod->getNumLinks()+1);
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
bod->clearForcesAndTorques();
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
}
}
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}