mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 14:10:11 +00:00
separate multibody position prediction into standalone function
This commit is contained in:
Xuchen Han
parent
96e8dcef0f
commit
436b6c6963
@ -1595,35 +1595,167 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
}
|
||||
void btMultiBody::predictPositionsMultiDof(btScalar dt)
|
||||
{
|
||||
stepPositionsMultiDof(dt, 0, 0, true);
|
||||
int num_links = getNumLinks();
|
||||
// step position by adding dt * velocity
|
||||
//btVector3 v = getBaseVel();
|
||||
//m_basePos += dt * v;
|
||||
//
|
||||
btScalar *pBasePos;
|
||||
btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
||||
|
||||
// reset to current position
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
m_basePos_interpolate[i] = m_basePos[i];
|
||||
}
|
||||
pBasePos = m_basePos_interpolate;
|
||||
|
||||
pBasePos[0] += dt * pBaseVel[0];
|
||||
pBasePos[1] += dt * pBaseVel[1];
|
||||
pBasePos[2] += dt * pBaseVel[2];
|
||||
|
||||
///////////////////////////////
|
||||
//local functor for quaternion integration (to avoid error prone redundancy)
|
||||
struct
|
||||
{
|
||||
//"exponential map" based on btTransformUtil::integrateTransform(..)
|
||||
void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
|
||||
{
|
||||
//baseBody => quat is alias and omega is global coor
|
||||
//!baseBody => quat is alibi and omega is local coor
|
||||
|
||||
btVector3 axis;
|
||||
btVector3 angvel;
|
||||
|
||||
if (!baseBody)
|
||||
angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
|
||||
else
|
||||
angvel = omega;
|
||||
|
||||
btScalar fAngle = angvel.length();
|
||||
//limit the angular motion
|
||||
if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
|
||||
{
|
||||
fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
|
||||
}
|
||||
|
||||
if (fAngle < btScalar(0.001))
|
||||
{
|
||||
// use Taylor's expansions of sync function
|
||||
axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
|
||||
}
|
||||
else
|
||||
{
|
||||
// sync(fAngle) = sin(c*fAngle)/t
|
||||
axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
|
||||
}
|
||||
|
||||
if (!baseBody)
|
||||
quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
|
||||
else
|
||||
quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
|
||||
//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
|
||||
|
||||
quat.normalize();
|
||||
}
|
||||
} pQuatUpdateFun;
|
||||
///////////////////////////////
|
||||
|
||||
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
|
||||
//
|
||||
btScalar *pBaseQuat;
|
||||
|
||||
// reset to current orientation
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
m_baseQuat_interpolate[i] = m_baseQuat[i];
|
||||
}
|
||||
pBaseQuat = m_baseQuat_interpolate;
|
||||
|
||||
btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
||||
//
|
||||
btQuaternion baseQuat;
|
||||
baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
||||
btVector3 baseOmega;
|
||||
baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
||||
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
||||
pBaseQuat[0] = baseQuat.x();
|
||||
pBaseQuat[1] = baseQuat.y();
|
||||
pBaseQuat[2] = baseQuat.z();
|
||||
pBaseQuat[3] = baseQuat.w();
|
||||
|
||||
// Finally we can update m_jointPos for each of the m_links
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
btScalar *pJointPos;
|
||||
pJointPos = &m_links[i].m_jointPos_interpolate[0];
|
||||
|
||||
btScalar *pJointVel = getJointVelMultiDof(i);
|
||||
|
||||
switch (m_links[i].m_jointType)
|
||||
{
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
//reset to current pos
|
||||
pJointPos[0] = m_links[i].m_jointPos[0];
|
||||
btScalar jointVel = pJointVel[0];
|
||||
pJointPos[0] += dt * jointVel;
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
//reset to current pos
|
||||
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
pJointPos[i] = m_links[i].m_jointPos[i];
|
||||
}
|
||||
|
||||
btVector3 jointVel;
|
||||
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
||||
btQuaternion jointOri;
|
||||
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
||||
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
||||
pJointPos[0] = jointOri.x();
|
||||
pJointPos[1] = jointOri.y();
|
||||
pJointPos[2] = jointOri.z();
|
||||
pJointPos[3] = jointOri.w();
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
pJointPos[i] = m_links[i].m_jointPos[i];
|
||||
}
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
|
||||
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
||||
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
||||
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
||||
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
m_links[i].updateInterpolationCacheMultiDof();
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict)
|
||||
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
|
||||
{
|
||||
int num_links = getNumLinks();
|
||||
// step position by adding dt * velocity
|
||||
//btVector3 v = getBaseVel();
|
||||
//m_basePos += dt * v;
|
||||
//
|
||||
btScalar *pBasePos;
|
||||
btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
|
||||
btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
||||
|
||||
if (!predict)
|
||||
{
|
||||
pBasePos = (pq ? &pq[4] : m_basePos);
|
||||
} //
|
||||
else
|
||||
{
|
||||
// reset to current position
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
m_basePos_interpolate[i] = m_basePos[i];
|
||||
}
|
||||
pBasePos = m_basePos_interpolate;
|
||||
}
|
||||
|
||||
|
||||
|
||||
pBasePos[0] += dt * pBaseVel[0];
|
||||
pBasePos[1] += dt * pBaseVel[1];
|
||||
pBasePos[2] += dt * pBaseVel[2];
|
||||
@ -1677,18 +1809,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
|
||||
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
|
||||
//
|
||||
btScalar *pBaseQuat;
|
||||
if (!predict)
|
||||
pBaseQuat = pq ? pq : m_baseQuat;
|
||||
else
|
||||
{
|
||||
// reset to current orientation
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
m_baseQuat_interpolate[i] = m_baseQuat[i];
|
||||
}
|
||||
pBaseQuat = m_baseQuat_interpolate;
|
||||
}
|
||||
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
|
||||
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
||||
//
|
||||
btQuaternion baseQuat;
|
||||
@ -1714,10 +1835,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
btScalar *pJointPos;
|
||||
if (!predict)
|
||||
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
|
||||
else
|
||||
pJointPos = &m_links[i].m_jointPos_interpolate[0];
|
||||
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
|
||||
|
||||
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
|
||||
|
||||
@ -1727,10 +1845,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
//reset to current pos
|
||||
if (predict)
|
||||
{
|
||||
pJointPos[0] = m_links[i].m_jointPos[0];
|
||||
}
|
||||
btScalar jointVel = pJointVel[0];
|
||||
pJointPos[0] += dt * jointVel;
|
||||
break;
|
||||
@ -1738,11 +1852,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
//reset to current pos
|
||||
if (predict)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
pJointPos[i] = m_links[i].m_jointPos[i];
|
||||
}
|
||||
btVector3 jointVel;
|
||||
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
||||
btQuaternion jointOri;
|
||||
@ -1756,11 +1865,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
if (predict)
|
||||
{
|
||||
for (int i = 0; i < 3; ++i)
|
||||
pJointPos[i] = m_links[i].m_jointPos[i];
|
||||
}
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
|
||||
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
||||
@ -1775,7 +1879,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
}
|
||||
}
|
||||
|
||||
m_links[i].updateCacheMultiDof(pq, predict);
|
||||
m_links[i].updateCacheMultiDof(pq);
|
||||
|
||||
if (pq)
|
||||
pq += m_links[i].m_posVarCount;
|
||||
|
||||
@ -435,7 +435,7 @@ public:
|
||||
}
|
||||
|
||||
// timestep the positions (given current velocities).
|
||||
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false);
|
||||
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
|
||||
|
||||
// predict the positions
|
||||
void predictPositionsMultiDof(btScalar dt);
|
||||
|
||||
@ -36,7 +36,7 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
|
||||
void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
|
||||
{
|
||||
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
|
||||
integrateMultiBodyTransforms(timeStep, /*predict = */ true);
|
||||
predictMultiBodyTransforms(timeStep);
|
||||
|
||||
}
|
||||
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
|
||||
@ -791,7 +791,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||
integrateMultiBodyTransforms(timeStep);
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict)
|
||||
void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
|
||||
{
|
||||
BT_PROFILE("btMultiBody stepPositions");
|
||||
//integrate and update the Featherstone hierarchies
|
||||
@ -815,28 +815,21 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b
|
||||
int nLinks = bod->getNumLinks();
|
||||
|
||||
///base + num m_links
|
||||
if (!predict)
|
||||
{
|
||||
if (!bod->isPosUpdated())
|
||||
bod->stepPositionsMultiDof(timeStep);
|
||||
else
|
||||
{
|
||||
btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
|
||||
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
|
||||
|
||||
bod->stepPositionsMultiDof(1, 0, pRealBuf);
|
||||
bod->setPosUpdated(false);
|
||||
}
|
||||
}
|
||||
if (!bod->isPosUpdated())
|
||||
bod->stepPositionsMultiDof(timeStep);
|
||||
else
|
||||
bod->predictPositionsMultiDof(timeStep);
|
||||
{
|
||||
btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
|
||||
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
|
||||
|
||||
bod->stepPositionsMultiDof(1, 0, pRealBuf);
|
||||
bod->setPosUpdated(false);
|
||||
}
|
||||
|
||||
|
||||
m_scratch_world_to_local.resize(nLinks + 1);
|
||||
m_scratch_local_origin.resize(nLinks + 1);
|
||||
if (predict)
|
||||
bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
|
||||
else
|
||||
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
|
||||
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -845,6 +838,40 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
|
||||
{
|
||||
BT_PROFILE("btMultiBody stepPositions");
|
||||
//integrate and update the Featherstone hierarchies
|
||||
|
||||
for (int b = 0; b < m_multiBodies.size(); b++)
|
||||
{
|
||||
btMultiBody* bod = m_multiBodies[b];
|
||||
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)
|
||||
{
|
||||
int nLinks = bod->getNumLinks();
|
||||
bod->predictPositionsMultiDof(timeStep);
|
||||
m_scratch_world_to_local.resize(nLinks + 1);
|
||||
m_scratch_local_origin.resize(nLinks + 1);
|
||||
bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
|
||||
}
|
||||
else
|
||||
{
|
||||
bod->clearVelocities();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
|
||||
{
|
||||
m_multiBodyConstraints.push_back(constraint);
|
||||
|
||||
@ -96,7 +96,8 @@ public:
|
||||
virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
|
||||
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false);
|
||||
void integrateMultiBodyTransforms(btScalar timeStep);
|
||||
void predictMultiBodyTransforms(btScalar timeStep);
|
||||
|
||||
virtual void predictUnconstraintMotion(btScalar timeStep);
|
||||
virtual void debugDrawWorld();
|
||||
|
||||
@ -191,16 +191,11 @@ struct btMultibodyLink
|
||||
}
|
||||
|
||||
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
||||
void updateCacheMultiDof(btScalar *pq = 0, bool predict = false)
|
||||
void updateCacheMultiDof(btScalar *pq = 0)
|
||||
{
|
||||
btScalar *pJointPos;
|
||||
|
||||
if (!predict)
|
||||
pJointPos = (pq ? pq : &m_jointPos[0]);
|
||||
else
|
||||
pJointPos = &m_jointPos_interpolate[0];
|
||||
btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis;
|
||||
btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector;
|
||||
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
|
||||
btQuaternion& cachedRot = m_cachedRotParentToThis;
|
||||
btVector3& cachedVector =m_cachedRVector;
|
||||
switch (m_jointType)
|
||||
{
|
||||
case eRevolute:
|
||||
@ -245,6 +240,57 @@ struct btMultibodyLink
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void updateInterpolationCacheMultiDof()
|
||||
{
|
||||
btScalar *pJointPos = &m_jointPos_interpolate[0];
|
||||
|
||||
btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
|
||||
btVector3& cachedVector = m_cachedRVector_interpolate;
|
||||
switch (m_jointType)
|
||||
{
|
||||
case eRevolute:
|
||||
{
|
||||
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case ePrismatic:
|
||||
{
|
||||
// m_cachedRotParentToThis never changes, so no need to update
|
||||
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
|
||||
|
||||
break;
|
||||
}
|
||||
case eSpherical:
|
||||
{
|
||||
cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
|
||||
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case ePlanar:
|
||||
{
|
||||
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||
cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case eFixed:
|
||||
{
|
||||
cachedRot = m_zeroRotParentToThis;
|
||||
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//invalid type
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_LINK_H
|
||||
|
||||
Loading…
Reference in New Issue
Block a user