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

first experiments with RK4

Browse Source
This commit is contained in:
kubas
2014-01-09 00:56:46 +01:00
parent cb556f9525
commit e5372f3712
5 changed files with 223 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -100,7 +100,9 @@ btMultiBody::btMultiBody(int n_links,
m_useGyroTerm(true),
m_maxAppliedImpulse(1000.f),
m_hasSelfCollision(true),
m_dofCount(n_links)
m_dofCount(n_links),
__posUpdated(false),
m_posVarCnt(-1) //-1 => not calculated yet/invalid
{
m_links.resize(n_links);
@@ -132,7 +134,9 @@ btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass,
m_useGyroTerm(true),
m_maxAppliedImpulse(1000.f),
m_hasSelfCollision(true),
m_dofCount(n_dofs)
m_dofCount(n_dofs),
__posUpdated(false),
m_posVarCnt(-1) //-1 => not calculated yet/invalid
{
m_links.resize(n_links);
@@ -1264,7 +1268,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular()));
//
zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
////clamp parent's omega
//btScalar parOmegaMod = temp.length();
//btScalar parOmegaModMax = 1000;
@@ -1272,6 +1276,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
// temp *= parOmegaModMax / parOmegaMod;
//zeroAccSpatFrc[i+1].addLinear(temp);
//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
//temp = spatCoriolisAcc[i].getLinear();
//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
@@ -1472,7 +1478,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
/////////////////
// Final step: add the accelerations (times dt) to the velocities.
applyDeltaVeeMultiDof(output, dt);
if(dt > 0.)
applyDeltaVeeMultiDof(output, dt);
/////
//btScalar angularThres = 1;
@@ -1501,38 +1508,38 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
//}
/////
///////////////////
if(useGlobalVelocities)
{
for (int i = 0; i < num_links; ++i)
{
const int parent = m_links[i].m_parent;
//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
fromWorld.m_rotMat = rot_from_world[i+1];
// vhat_i = i_xhat_p(i) * vhat_p(i)
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
//nice alternative below (using operator *) but it generates temps
/////////////////////////////////////////////////////////////
/////////////////////
//if(useGlobalVelocities)
//{
// for (int i = 0; i < num_links; ++i)
// {
// const int parent = m_links[i].m_parent;
// //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
// //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
//
// fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
// fromWorld.m_rotMat = rot_from_world[i+1];
//
// // vhat_i = i_xhat_p(i) * vhat_p(i)
// fromParent.transform(spatVel[parent+1], spatVel[i+1]);
// //nice alternative below (using operator *) but it generates temps
// /////////////////////////////////////////////////////////////
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
spatJointVel.setZero();
// // now set vhat_i to its true value by doing
// // vhat_i += qidot * shat_i
// spatJointVel.setZero();
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
// remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
spatVel[i+1] += spatJointVel;
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
//
// // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
// spatVel[i+1] += spatJointVel;
fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
}
}
// fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
// fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
// }
//}
}
#endif
@@ -2464,8 +2471,8 @@ void btMultiBody::stepPositions(btScalar dt)
}
}
void btMultiBody::stepPositionsMultiDof(btScalar dt)
{
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
{
int num_links = getNumLinks();
// step position by adding dt * velocity
btVector3 v = getBaseVel();
@@ -2523,40 +2530,48 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt)
// Finally we can update m_jointPos for each of the m_links
for (int i = 0; i < num_links; ++i)
{
btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
switch(m_links[i].m_jointType)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
btScalar jointVel = getJointVelMultiDof(i)[0];
m_links[i].m_jointPos[0] += dt * jointVel;
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
btScalar *pJointVel = getJointVelMultiDof(i);
//btScalar *pJointVel = getJointVelMultiDof(i);
static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
static btQuaternion jointOri; jointOri.setValue(m_links[i].m_jointPos[0], m_links[i].m_jointPos[1], m_links[i].m_jointPos[2], m_links[i].m_jointPos[3]);
static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
m_links[i].m_jointPos[0] = jointOri.x(); m_links[i].m_jointPos[1] = jointOri.y(); m_links[i].m_jointPos[2] = jointOri.z(); m_links[i].m_jointPos[3] = jointOri.w();
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
break;
}
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case btMultibodyLink::ePlanar:
{
m_links[i].m_jointPos[0] += dt * getJointVelMultiDof(i)[0];
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), m_links[i].m_jointPos[0]), q0_coors_qd1qd2);
m_links[i].m_jointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
m_links[i].m_jointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
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;
}
#endif
}
m_links[i].updateCacheMultiDof();
m_links[i].updateCacheMultiDof(pq);
if(pq)
pq += m_links[i].m_posVarCount;
if(pqd)
pqd += m_links[i].m_dofCount;
}
}

19
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -138,6 +138,7 @@ public:
int getNumLinks() const { return m_links.size(); }
int getNumDofs() const { return m_dofCount; }
int getNumPosVars() const { return m_posVarCnt; }
btScalar getBaseMass() const { return m_baseMass; }
const btVector3 & getBaseInertia() const { return m_baseInertia; }
btScalar getLinkMass(int i) const;
@@ -375,7 +376,7 @@ public:
// timestep the positions (given current velocities).
void stepPositions(btScalar dt);
void stepPositionsMultiDof(btScalar dt);
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
//
@@ -481,6 +482,8 @@ public:
bool isMultiDof() { return m_isMultiDof; }
void forceMultiDof();
bool __posUpdated;
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@@ -492,7 +495,17 @@ private:
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
#endif
void updateLinksDofOffsets() { int dofOffset = 0; for(int bidx = 0; bidx < m_links.size(); ++bidx) { m_links[bidx].m_dofOffset = dofOffset; dofOffset += m_links[bidx].m_dofCount; } }
void updateLinksDofOffsets()
{
int dofOffset = 0, cfgOffset = 0;
for(int bidx = 0; bidx < m_links.size(); ++bidx)
{
m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
}
m_posVarCnt = cfgOffset;
}
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
@@ -511,7 +524,7 @@ private:
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
int m_dofCount;
int m_dofCount, m_posVarCnt;
//
// realBuf:

8
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -36,6 +36,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//if (iteration < constraint.m_overrideNumSolverIterations)
//resolveSingleConstraintRowGenericMultiBody(constraint);
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
}
//solve featherstone normal contact
@@ -44,6 +46,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
if (iteration < infoGlobal.m_numIterations)
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
}
//solve featherstone frictional contact
@@ -60,6 +65,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(frictionConstraint);
if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->__posUpdated = false;
if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->__posUpdated = false;
}
}
}

138
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
@@ -392,9 +394,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
delete m_solverMultiBodyIslandCallback;
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
@@ -464,8 +463,130 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bool rk = false;
bool doNotUpdatePos = false;
if(bod->isMultiDof())
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
{
if(!rk)
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
else
{
//
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*bod->getNumPosVars() + 8*bod->getNumDofs());
//convenience
btScalar *pMem = &scratch_r2[0];
btScalar *scratch_q0 = pMem; pMem += bod->getNumPosVars();
btScalar *scratch_qx = pMem; pMem += bod->getNumPosVars();
btScalar *scratch_qd0 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qd1 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qd2 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qd3 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qdd0 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qdd1 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qdd2 = pMem; pMem += bod->getNumDofs();
btScalar *scratch_qdd3 = pMem; pMem += bod->getNumDofs();
btAssert((pMem - (2*bod->getNumPosVars() + 8*bod->getNumDofs())) == &scratch_r2[0]);
//copy q0 to scratch_q0 and qd0 to scratch_qd0
for(int link = 0; link < bod->getNumLinks(); ++link)
{
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
scratch_q0[bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
for(int dof = 0; dof < bod->getLink(link).m_dofCount; ++dof)
scratch_qd0[bod->getLink(link).m_dofOffset + dof] = bod->getVelocityVector()[6 + bod->getLink(link).m_dofOffset + dof];
}
auto pResetQx = [&scratch_qx, &scratch_q0, &bod]()
{
for(int dof = 0; dof < bod->getNumPosVars(); ++dof)
scratch_qx[dof] = scratch_q0[dof];
};
auto pEulerIntegrate = [](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];
};
//
auto pCopyToVelocitVector = [](btMultiBody *pBody, const btScalar *pData)
{
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
for(int i = 0; i < pBody->getNumDofs(); ++i)
pVel[6+i] = pData[i];
};
//
auto pCopy = [](const btScalar *pSrc, btScalar *pDst, int start, int size)
{
for(int i = 0; i < size; ++i)
pDst[i] = pSrc[start + i];
};
//
btScalar h = solverInfo.m_timeStep;
//calc qdd0 from: q0 & qd0
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(&scratch_r[0], scratch_qdd0, bod->getNumDofs() + 6, bod->getNumDofs());
//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, bod->getNumDofs());
//
//calc qdd1 from: q1 & qd1
pCopyToVelocitVector(bod, scratch_qd1);
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(&scratch_r[0], scratch_qdd1, bod->getNumDofs() + 6, bod->getNumDofs());
//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, bod->getNumDofs());
//
//calc qdd2 from: q2 & qd2
pCopyToVelocitVector(bod, scratch_qd2);
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(&scratch_r[0], scratch_qdd2, bod->getNumDofs() + 6, bod->getNumDofs());
//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, bod->getNumDofs());
//
//calc qdd3 from: q3 & qd3
pCopyToVelocitVector(bod, scratch_qd3);
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(&scratch_r[0], scratch_qdd3, bod->getNumDofs() + 6, bod->getNumDofs());
//
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
//btAlignedObjectArray<btScalar> res_qd; res_qd.resize(bod->getNumDofs());
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(bod->getNumDofs());
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(6+bod->getNumDofs());
for(int i = 0; i < bod->getNumDofs(); ++i)
{
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
delta_qd[6+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[6+i] = h*scratch_qdd0[i];
}
//
if(!doNotUpdatePos)
{
bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->__posUpdated = true;
}
//
pCopyToVelocitVector(bod, scratch_qd0);
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
//
//delete [] scratch_q;
}
}
else
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
@@ -512,9 +633,12 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
local_origin.resize(nLinks+1);
if(bod->isMultiDof())
bod->stepPositionsMultiDof(timeStep);
{
if(!bod->__posUpdated)
bod->stepPositionsMultiDof(timeStep);
}
else
bod->stepPositions(timeStep);
bod->stepPositions(timeStep);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();

16
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -416,7 +416,7 @@ struct btMultibodyLink
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
#endif
int m_dofOffset;
int m_dofOffset, m_cfgOffset;
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
@@ -471,13 +471,15 @@ struct btMultibodyLink
}
}
void updateCacheMultiDof()
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
switch(m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
@@ -485,13 +487,13 @@ struct btMultibodyLink
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(m_jointPos[0], m_jointPos[1], m_jointPos[2], -m_jointPos[3]) * m_zeroRotParentToThis;
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
@@ -499,8 +501,8 @@ struct btMultibodyLink
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-m_jointPos[0]), m_jointPos[1] * m_axesBottom[1] + m_jointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}