Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

separate multibody position prediction into standalone function

Browse Source
This commit is contained in:
Xuchen Han
2019-08-08 17:14:13 -07:00
parent 96e8dcef0f
commit 436b6c6963
5 changed files with 259 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -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;