diff options
author | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-08 17:14:13 -0700 |
---|---|---|
committer | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-08 17:14:13 -0700 |
commit | 436b6c6963d4639599908d4643d4b5f341f86f6c (patch) | |
tree | 935582d285c754bcb5aa9aa80dc1a0292975ddb5 | |
parent | 96e8dcef0f57c730ea1f268d6cd575dac22f5d7e (diff) | |
download | bullet3-436b6c6963d4639599908d4643d4b5f341f86f6c.tar.gz |
separate multibody position prediction into standalone function
5 files changed, 259 insertions, 81 deletions
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 1857bd55f..4d634b699 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index fe60af991..91b5c3edb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -435,7 +435,7 @@ public: } // timestep the positions (given current velocities). - void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false); + void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); // predict the positions void predictPositionsMultiDof(btScalar dt); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 2e6dbc440..1be76ad86 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -36,7 +36,7 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); - integrateMultiBodyTransforms(timeStep, /*predict = */ true); + predictMultiBodyTransforms(timeStep); } void btMultiBodyDynamicsWorld::calculateSimulationIslands() @@ -791,7 +791,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) integrateMultiBodyTransforms(timeStep); } -void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict) +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) { BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies @@ -815,28 +815,21 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b int nLinks = bod->getNumLinks(); ///base + num m_links - if (!predict) - { - if (!bod->isPosUpdated()) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->setPosUpdated(false); - } - } + if (!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); else - bod->predictPositionsMultiDof(timeStep); + { + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->setPosUpdated(false); + } + m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); - if (predict) - bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); - else - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); } else { @@ -845,6 +838,40 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b } } +void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep) +{ + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + + for (int b = 0; b < m_multiBodies.size(); b++) + { + btMultiBody* bod = m_multiBodies[b]; + bool isSleeping = false; + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + bod->predictPositionsMultiDof(timeStep); + m_scratch_world_to_local.resize(nLinks + 1); + m_scratch_local_origin.resize(nLinks + 1); + bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + } + else + { + bod->clearVelocities(); + } + } +} + void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) { m_multiBodyConstraints.push_back(constraint); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 04707bf2d..653ec36ca 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -96,7 +96,8 @@ public: virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); - void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false); + void integrateMultiBodyTransforms(btScalar timeStep); + void predictMultiBodyTransforms(btScalar timeStep); virtual void predictUnconstraintMotion(btScalar timeStep); virtual void debugDrawWorld(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index e7966b852..1cd6b8c07 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -191,16 +191,11 @@ struct btMultibodyLink } // routine to update m_cachedRotParentToThis and m_cachedRVector - void updateCacheMultiDof(btScalar *pq = 0, bool predict = false) + void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos; - - if (!predict) - pJointPos = (pq ? pq : &m_jointPos[0]); - else - pJointPos = &m_jointPos_interpolate[0]; - btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis; - btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector; + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector =m_cachedRVector; switch (m_jointType) { case eRevolute: @@ -245,6 +240,57 @@ struct btMultibodyLink } } } + + void updateInterpolationCacheMultiDof() + { + btScalar *pJointPos = &m_jointPos_interpolate[0]; + + btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate; + btVector3& cachedVector = m_cachedRVector_interpolate; + switch (m_jointType) + { + case eRevolute: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + case ePlanar: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); + + break; + } + case eFixed: + { + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + default: + { + //invalid type + btAssert(0); + } + } + } }; #endif //BT_MULTIBODY_LINK_H |