AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

RK4 for floating systems too

Browse Source
This commit is contained in:
kubas 2014-01-09 01:01:03 +01:00
parent c5594a5826
commit 66fdc1704b
2 changed files with 106 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -472,34 +472,46 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
else
{
//
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*bod->getNumPosVars() + 8*bod->getNumDofs());
int numDofs = bod->getNumDofs() + 6;
int numPosVars = bod->getNumPosVars() + 7;
btAlignedObjectArray<btScalar> 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<btScalar*>(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<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)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
btAlignedObjectArray<btScalar> 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<btScalar *>(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