diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 3fb41f89b..8e69ba679 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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; } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 048e3341c..8b3b33bc9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -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 m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; - int m_dofCount; + int m_dofCount, m_posVarCnt; // // realBuf: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 7826abd41..1e4004af3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -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; } } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index bdcd8c5f6..1e6d7a7fe 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -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 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(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 res_qd; res_qd.resize(bod->getNumDofs()); + btAlignedObjectArray delta_q; delta_q.resize(bod->getNumDofs()); + btAlignedObjectArray 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(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 7566bda53..af926f57f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -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; }