separate external force solve from constraint solve and eliminate damping in external force solve
This commit is contained in:
Xuchen Han
@@ -421,288 +421,7 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
|
|||||||
}
|
}
|
||||||
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||||
{
|
{
|
||||||
forwardKinematics();
|
solveExternalForces(solverInfo);
|
||||||
|
|
||||||
BT_PROFILE("solveConstraints");
|
|
||||||
|
|
||||||
clearMultiBodyConstraintForces();
|
|
||||||
|
|
||||||
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());
|
|
||||||
|
|
||||||
#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;
|
|
||||||
bool isConstraintPass = false;
|
|
||||||
{
|
|
||||||
if (!bod->isUsingRK4Integration())
|
|
||||||
{
|
|
||||||
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
|
|
||||||
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
|
|
||||||
getSolverInfo().m_jointFeedbackInWorldSpace,
|
|
||||||
getSolverInfo().m_jointFeedbackInJointFrame);
|
|
||||||
}
|
|
||||||
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,
|
|
||||||
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
|
||||||
getSolverInfo().m_jointFeedbackInJointFrame);
|
|
||||||
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,
|
|
||||||
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
|
||||||
getSolverInfo().m_jointFeedbackInJointFrame);
|
|
||||||
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,
|
|
||||||
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
|
||||||
getSolverInfo().m_jointFeedbackInJointFrame);
|
|
||||||
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,
|
|
||||||
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
|
||||||
getSolverInfo().m_jointFeedbackInJointFrame);
|
|
||||||
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,
|
|
||||||
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
|
||||||
getSolverInfo().m_jointFeedbackInJointFrame);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
|
|
||||||
bod->clearForcesAndTorques();
|
|
||||||
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
|
|
||||||
} //if (!isSleeping)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// solve all the constraints for this island
|
/// solve all the constraints for this island
|
||||||
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
|
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
|
||||||
@@ -760,6 +479,298 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
|
||||||
|
{
|
||||||
|
forwardKinematics();
|
||||||
|
|
||||||
|
BT_PROFILE("solveConstraints");
|
||||||
|
|
||||||
|
clearMultiBodyConstraintForces();
|
||||||
|
|
||||||
|
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());
|
||||||
|
|
||||||
|
#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;
|
||||||
|
bool isConstraintPass = false;
|
||||||
|
{
|
||||||
|
if (!bod->isUsingRK4Integration())
|
||||||
|
{
|
||||||
|
const btScalar linearDamp = bod->getLinearDamping();
|
||||||
|
// const btScalar angularDamp = bod->getAngularDamping();
|
||||||
|
bod->setLinearDamping(0);
|
||||||
|
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
|
||||||
|
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
|
||||||
|
getSolverInfo().m_jointFeedbackInWorldSpace,
|
||||||
|
getSolverInfo().m_jointFeedbackInJointFrame);
|
||||||
|
bod->setLinearDamping(linearDamp);
|
||||||
|
// bod->setAngularDamping(angularDamp);
|
||||||
|
}
|
||||||
|
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,
|
||||||
|
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
||||||
|
getSolverInfo().m_jointFeedbackInJointFrame);
|
||||||
|
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,
|
||||||
|
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
||||||
|
getSolverInfo().m_jointFeedbackInJointFrame);
|
||||||
|
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,
|
||||||
|
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
||||||
|
getSolverInfo().m_jointFeedbackInJointFrame);
|
||||||
|
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,
|
||||||
|
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
||||||
|
getSolverInfo().m_jointFeedbackInJointFrame);
|
||||||
|
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,
|
||||||
|
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
|
||||||
|
getSolverInfo().m_jointFeedbackInJointFrame);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
|
||||||
|
bod->clearForcesAndTorques();
|
||||||
|
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
|
||||||
|
} //if (!isSleeping)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||||
{
|
{
|
||||||
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
|
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
|
||||||
|
|||||||
@@ -112,5 +112,7 @@ public:
|
|||||||
virtual void setConstraintSolver(btConstraintSolver* solver);
|
virtual void setConstraintSolver(btConstraintSolver* solver);
|
||||||
virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
|
virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
|
||||||
|
|
||||||
|
virtual void solveExternalForces(btContactSolverInfo& solverInfo);
|
||||||
|
|
||||||
};
|
};
|
||||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||||
|
|||||||
Reference in New Issue
Block a user