diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 4b06aec7b..e0dac8c3a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -306,8 +306,8 @@ void btMultiBody::finalizeMultiDof() { btAssert(m_isMultiDof); - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + m_posVarCnt); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) updateLinksDofOffsets(); } @@ -1058,7 +1058,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, scratch_v.resize(8*num_links + 6); scratch_m.resize(4*num_links + 4); - btScalar * r_ptr = &scratch_r[0]; + //btScalar * r_ptr = &scratch_r[0]; btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results btVector3 * v_ptr = &scratch_v[0]; @@ -1188,14 +1188,14 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) // mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof]; - // btScalar angvel = sqrt(mod2); - // btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075; - // btScalar step = 1; //dt - // if (angvel*step > maxAngVel) - // { + // btScalar angvel = sqrt(mod2); + // btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075; + // btScalar step = 1; //dt + // if (angvel*step > maxAngVel) + // { // btScalar * qd = getJointVelMultiDof(i); // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - // qd[dof] *= (maxAngVel/step) /angvel; + // qd[dof] *= (maxAngVel/step) /angvel; // } //} @@ -2460,8 +2460,15 @@ 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; + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + 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) + // + 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) @@ -2510,7 +2517,28 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } pQuatUpdateFun; /////////////////////////////// - pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + // + 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) + // + static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + static 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(); + + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + + if(pq) + pq += 7; + if(pqd) + pqd += 6; // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) @@ -2529,7 +2557,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::eSpherical: { - //btScalar *pJointVel = getJointVelMultiDof(i); static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); pQuatUpdateFun(jointVel, jointOri, false, dt); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cf5c5edc5..9c2e0387d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -472,34 +472,46 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) else { // - btAlignedObjectArray scratch_r2; scratch_r2.resize(2*bod->getNumPosVars() + 8*bod->getNumDofs()); + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); //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]); + 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[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]; + 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]; + //// auto pResetQx = [&scratch_qx, &scratch_q0, &bod]() { - for(int dof = 0; dof < bod->getNumPosVars(); ++dof) + for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) scratch_qx[dof] = scratch_q0[dof]; }; @@ -509,12 +521,12 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) pVal[i] = pCurVal[i] + dt * pDer[i]; }; // - auto pCopyToVelocitVector = [](btMultiBody *pBody, const btScalar *pData) + auto pCopyToVelocityVector = [](btMultiBody *pBody, const btScalar *pData) { btScalar *pVel = const_cast(pBody->getVelocityVector()); - for(int i = 0; i < pBody->getNumDofs(); ++i) - pVel[6+i] = pData[i]; + for(int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; }; // auto pCopy = [](const btScalar *pSrc, btScalar *pDst, int start, int size) @@ -524,72 +536,77 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) }; // - btScalar h = solverInfo.m_timeStep; - //calc qdd0 from: q0 & qd0 + btScalar h = solverInfo.m_timeStep; + #define output &scratch_r[bod->getNumDofs()] + //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()); + 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, bod->getNumDofs()); + pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); // - //calc qdd1 from: q1 & qd1 - pCopyToVelocitVector(bod, scratch_qd1); + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd1, bod->getNumDofs() + 6, bod->getNumDofs()); + 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, bod->getNumDofs()); + pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); // //calc qdd2 from: q2 & qd2 - pCopyToVelocitVector(bod, scratch_qd2); + pCopyToVelocityVector(bod, scratch_qd2); bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd2, bod->getNumDofs() + 6, bod->getNumDofs()); + 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, bod->getNumDofs()); + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); // //calc qdd3 from: q3 & qd3 - pCopyToVelocitVector(bod, scratch_qd3); + pCopyToVelocityVector(bod, scratch_qd3); bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd3, bod->getNumDofs() + 6, bod->getNumDofs()); + 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 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) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray delta_q; delta_q.resize(numDofs); + btAlignedObjectArray 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[6+i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[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[6+i] = h*scratch_qdd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; } - + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); // if(!doNotUpdatePos) { btScalar *pRealBuf = const_cast(bod->getVelocityVector()); pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - for(int i = 0; i < bod->getNumPosVars(); ++i) + for(int i = 0; i < numDofs; ++i) pRealBuf[i] = delta_q[i]; //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->__posUpdated = true; + bod->__posUpdated = true; } - // - pCopyToVelocitVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - //delete [] scratch_q; + + //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->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); + } + } } else