diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone')
9 files changed, 1104 insertions, 680 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index 3e210d7520..bdaa473476 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -100,6 +100,8 @@ btMultiBody::btMultiBody(int n_links, m_baseName(0), m_basePos(0, 0, 0), m_baseQuat(0, 0, 0, 1), + m_basePos_interpolate(0, 0, 0), + m_baseQuat_interpolate(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -449,6 +451,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const return m_links[i].m_cachedRotParentToThis; } +const btVector3 &btMultiBody::getInterpolateRVector(int i) const +{ + return m_links[i].m_cachedRVector_interpolate; +} + +const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const +{ + return m_links[i].m_cachedRotParentToThis_interpolate; +} + btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const { btAssert(i >= -1); @@ -1581,6 +1593,158 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar //printf("]\n"); ///////////////// } +void btMultiBody::predictPositionsMultiDof(btScalar dt) +{ + 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 j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + + 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 j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + 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) { @@ -1589,9 +1753,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //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) - // + 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]; @@ -1645,7 +1809,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; + 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; @@ -1670,7 +1834,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // 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 *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); switch (m_links[i].m_jointType) @@ -1678,12 +1844,14 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { + //reset to current pos btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { + //reset to current pos btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); btQuaternion jointOri; @@ -1974,6 +2142,7 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); getBaseCollider()->setWorldTransform(tr); + getBaseCollider()->setInterpolationWorldTransform(tr); } for (int k = 0; k < getNumLinks(); k++) @@ -2002,10 +2171,62 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); col->setWorldTransform(tr); + col->setInterpolationWorldTransform(tr); } } } +void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) +{ + world_to_local.resize(getNumLinks() + 1); + local_origin.resize(getNumLinks() + 1); + + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getBaseCollider()->setInterpolationWorldTransform(tr); + } + + for (int k = 0; k < getNumLinks(); k++) + { + const int parent = getParent(k); + world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k))); + } + + for (int m = 0; m < getNumLinks(); m++) + { + btMultiBodyLinkCollider *col = getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link + 1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + col->setInterpolationWorldTransform(tr); + } + } +} + int btMultiBody::calculateSerializeBufferSize() const { int sz = sizeof(btMultiBodyData); diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h index c0b0d003be..afed669a7b 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h @@ -193,12 +193,24 @@ public: const btQuaternion &getWorldToBaseRot() const { return m_baseQuat; - } // rotates world vectors into base frame + } + + const btVector3 &getInterpolateBasePos() const + { + return m_basePos_interpolate; + } // in world frame + const btQuaternion &getInterpolateWorldToBaseRot() const + { + return m_baseQuat_interpolate; + } + + // rotates world vectors into base frame btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame void setBasePos(const btVector3 &pos) { m_basePos = pos; + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -224,6 +236,7 @@ public: void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + m_baseQuat_interpolate = rot; } void setBaseOmega(const btVector3 &omega) { @@ -260,6 +273,11 @@ public: { return &m_realBuf[0]; } + + const btScalar *getDeltaVelocityVector() const + { + return &m_deltaV[0]; + } /* btScalar * getVelocityVector() { return &real_buf[0]; @@ -273,6 +291,8 @@ public: const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. // // transform vectors in local frame of link i to world frame (or vice versa) @@ -421,6 +441,9 @@ public: // timestep the positions (given current velocities). void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + + // predict the positions + void predictPositionsMultiDof(btScalar dt); // // contacts @@ -581,6 +604,7 @@ public: void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); virtual int calculateSerializeBufferSize() const; @@ -664,7 +688,9 @@ private: const char *m_baseName; //memory needs to be manager by user! btVector3 m_basePos; // position of COM of base (world frame) + btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) btQuaternion m_baseQuat; // rotates world points into base frame + btQuaternion m_baseQuat_interpolate; btScalar m_baseMass; // mass of the base btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index e17ab94d98..d7ed05ce57 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -342,40 +342,6 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra solverConstraint.m_friction = 0.f; //cp.m_combinedFriction; } - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 23e163f0e8..ffae5300f0 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -22,6 +22,8 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "LinearMath/btQuickprof.h" +#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h" +#include "LinearMath/btScalar.h" btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { @@ -491,11 +493,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt return deltaVel; } -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; @@ -781,48 +779,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } - ///warm starting (or zero if disabled) - //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse); - - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); - } - else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse); - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); - } - else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); - } - } - } - else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - { btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction @@ -874,6 +830,54 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; } + + if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) + { + if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor; + if (solverConstraint.m_appliedImpulse < 0) + solverConstraint.m_appliedImpulse = 0; + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); + + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); + } + } + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + } } void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, @@ -944,13 +948,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); - btVector3 torqueAxis0 = -constraintNormal; + btVector3 torqueAxis0 = constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); } else { - btVector3 torqueAxis0 = -constraintNormal; + btVector3 torqueAxis0 = constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); @@ -986,13 +990,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); - btVector3 torqueAxis1 = constraintNormal; + btVector3 torqueAxis1 = -constraintNormal; solverConstraint.m_relpos2CrossNormal = torqueAxis1; solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); } else { - btVector3 torqueAxis1 = constraintNormal; + btVector3 torqueAxis1 = -constraintNormal; solverConstraint.m_relpos2CrossNormal = torqueAxis1; solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); @@ -1130,7 +1134,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu } } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); @@ -1161,7 +1165,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); return solverConstraint; } @@ -1297,7 +1301,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction); + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction); // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -1371,13 +1375,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* { applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) @@ -1388,26 +1392,27 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; - } + } #endif //ENABLE_FRICTION } + else + { + // Reset quantities related to warmstart as 0. + cp.m_appliedImpulse = 0; + cp.m_prevRHS = 0; + } } } void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { - //btPersistentManifold* manifold = 0; - for (int i = 0; i < numManifolds; i++) { btPersistentManifold* manifold = manifoldPtr[i]; @@ -1434,6 +1439,51 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal); } + + // Warmstart for noncontact constraints + if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) + { + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) + { + btMultiBodySolverConstraint& solverConstraint = + m_multiBodyNonContactConstraints[i]; + solverConstraint.m_appliedImpulse = + solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) * + infoGlobal.m_articulatedWarmstartingFactor; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + int ndofA = multiBodyA->getNumDofs() + 6; + btScalar* deltaV = + &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + btScalar impulse = solverConstraint.m_appliedImpulse; + multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + if (multiBodyB) + { + int ndofB = multiBodyB->getNumDofs() + 6; + btScalar* deltaV = + &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + btScalar impulse = solverConstraint.m_appliedImpulse; + multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + } + } + } + else + { + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) + { + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; + solverConstraint.m_appliedImpulse = 0; + } + } } btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) @@ -1556,7 +1606,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep); } - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { BT_PROFILE("warm starting write back"); for (int j = 0; j < numPoolConstraints; j++) @@ -1565,6 +1615,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; + pt->m_prevRHS = solverConstraint.m_rhs; pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); @@ -1576,9 +1627,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO pt->m_appliedImpulseLateral2 = 0; } } - - //do a callback here? } + #if 0 //multibody joint feedback { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index abf5718839..f584360e2b 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -49,7 +49,7 @@ protected: void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, @@ -66,7 +66,9 @@ protected: void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + const btScalar& appliedImpulse, + btManifoldPoint& cp, + const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); @@ -82,7 +84,6 @@ protected: void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 1131e5378c..cd1bad089e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) m_multiBodies.remove(body); } +void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + predictMultiBodyTransforms(timeStep); + +} void btMultiBodyDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); @@ -163,218 +169,6 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) btDiscreteDynamicsWorld::updateActivationState(timeStep); } -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); - return islandId; -} - -class btSortConstraintOnIslandPredicate2 -{ -public: - bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const - { - int rIslandId0, lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } -}; - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) -{ - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - islandId = islandTagA >= 0 ? islandTagA : islandTagB; - return islandId; -} - -class btSortMultiBodyConstraintOnIslandPredicate -{ -public: - bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const - { - int rIslandId0, lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - - -struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback -{ - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - btAlignedObjectArray<btCollisionObject*> m_bodies; - btAlignedObjectArray<btPersistentManifold*> m_manifolds; - btAlignedObjectArray<btTypedConstraint*> m_constraints; - btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; - - btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData; - - MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - : m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - } - - MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - m_islandAnalyticsData.clear(); - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - - void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) - { - m_solver = solver; - } - - virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) - { - if (islandId < 0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_solverInfo->m_reportSolverAnalytics&1) - { - m_solver->m_analyticsData.m_islandId = islandId; - m_islandAnalyticsData.push_back(m_solver->m_analyticsData); - } - } - else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i = 0; i < m_numConstraints; i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (; i < m_numConstraints; i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - for (i = 0; i < m_numMultiBodyConstraints; i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; - break; - } - } - //count the number of multi body constraints in this island - for (; i < m_numMultiBodyConstraints; i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - numCurMultiBodyConstraints++; - } - } - - //if (m_solverInfo->m_minimumSolverBatchSize<=1) - //{ - // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - //} else - { - for (i = 0; i < numBodies; i++) - m_bodies.push_back(bodies[i]); - for (i = 0; i < numManifolds; i++) - m_manifolds.push_back(manifolds[i]); - for (i = 0; i < numCurConstraints; i++) - m_constraints.push_back(startConstraint[i]); - - for (i = 0; i < numCurMultiBodyConstraints; i++) - m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); - - if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(islandId); - } - else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints(int islandId=-1) - { - 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; - - //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); - if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) - { - m_solver->m_analyticsData.m_islandId = islandId; - m_islandAnalyticsData.push_back(m_solver->m_analyticsData); - } - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } -}; - void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const { islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData; @@ -421,350 +215,364 @@ void btMultiBodyDynamicsWorld::forwardKinematics() } void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - forwardKinematics(); - - BT_PROFILE("solveConstraints"); - - clearMultiBodyConstraintForces(); - - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + solveExternalForces(solverInfo); + buildIslands(); + solveInternalConstraints(solverInfo); +} - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; +void btMultiBodyDynamicsWorld::buildIslands() +{ + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); +} - m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); +void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) +{ + /// solve all the constraints for this island + m_solverMultiBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + 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) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } +} +void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) +{ + forwardKinematics(); + + BT_PROFILE("solveConstraints"); + + clearMultiBodyConstraintForces(); + + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + + m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - { - BT_PROFILE("btMultiBody addForce"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - 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) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - } //if (!isSleeping) - } - } + { + BT_PROFILE("btMultiBody addForce"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + 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) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + } //if (!isSleeping) + } + } #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - 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) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool doNotUpdatePos = false; + + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + 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) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool doNotUpdatePos = false; bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - // - 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 += 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[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]; - //// - struct - { - btMultiBody* bod; - btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - } - } pResetQx = {bod, scratch_qx, scratch_q0}; - // - struct - { - void operator()(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]; - } - - } pEulerIntegrate; - // - struct - { - void operator()(btMultiBody* pBody, const btScalar* pData) - { - btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector()); - - for (int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - } - } pCopyToVelocityVector; - // - struct - { - void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) - { - for (int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - } - } pCopy; - // - - btScalar h = solverInfo.m_timeStep; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + // + 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 += 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[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]; + //// + struct + { + btMultiBody* bod; + btScalar *scratch_qx, *scratch_q0; + + void operator()() + { + for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } + } pResetQx = {bod, scratch_qx, scratch_q0}; + // + struct + { + void operator()(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]; + } + + } pEulerIntegrate; + // + struct + { + void operator()(btMultiBody* pBody, const btScalar* pData) + { + btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector()); + + for (int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + } + } pCopyToVelocityVector; + // + struct + { + void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) + { + for (int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } + } pCopy; + // + + btScalar h = solverInfo.m_timeStep; #define output &m_scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - 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, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - 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, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - 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, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - 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> 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[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[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 < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); - } - - //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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - + //calc qdd0 from: q0 & qd0 + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + 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, numDofs); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + 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, numDofs); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocityVector(bod, scratch_qd2); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + 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, numDofs); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocityVector(bod, scratch_qd3); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + 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> 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[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[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 < numDofs; ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->setPosUpdated(true); + } + + //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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - bod->clearForcesAndTorques(); + bod->clearForcesAndTorques(); #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - } //if (!isSleeping) - } - } - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); - - m_solverMultiBodyIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - 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) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - if (bod->internalNeedsJointFeedback()) - { - if (!bod->isUsingRK4Integration()) - { - if (bod->internalNeedsJointFeedback()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - } - } - } - - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } + } //if (!isSleeping) + } + } } + void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); + integrateMultiBodyTransforms(timeStep); +} - { +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) +{ BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies @@ -787,31 +595,61 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) int nLinks = bod->getNumLinks(); ///base + num m_links + if (!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); + else + { + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - { - 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); + } - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->setPosUpdated(false); - } - } m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); - - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); } else { bod->clearVelocities(); } } - } +} + +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) @@ -1029,3 +867,8 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } } +// +//void btMultiBodyDynamicsWorld::setSplitIslands(bool split) +//{ +// m_islandManager->setSplitIslands(split); +//} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index e36c2f7aad..9ac46f4b64 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_MULTIBODY_DYNAMICS_WORLD_H #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h" #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY @@ -47,7 +48,7 @@ protected: virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void serializeMultiBodies(btSerializer* serializer); @@ -55,7 +56,9 @@ public: btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); virtual ~btMultiBodyDynamicsWorld(); - + + virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); virtual void removeMultiBody(btMultiBody* body); @@ -95,7 +98,10 @@ public: virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); - + void integrateMultiBodyTransforms(btScalar timeStep); + void predictMultiBodyTransforms(btScalar timeStep); + + virtual void predictUnconstraintMotion(btScalar timeStep); virtual void debugDrawWorld(); virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); @@ -110,6 +116,9 @@ public: virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); virtual void setConstraintSolver(btConstraintSolver* solver); virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const; - + + virtual void solveExternalForces(btContactSolverInfo& solverInfo); + virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); + void buildIslands(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h new file mode 100644 index 0000000000..3169b86e61 --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h @@ -0,0 +1,247 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H +#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H + +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; +} +class btSortConstraintOnIslandPredicate2 +{ +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId = islandTagA >= 0 ? islandTagA : islandTagB; + return islandId; +} + +class btSortMultiBodyConstraintOnIslandPredicate +{ +public: + bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btCollisionObject*> m_softBodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; + + btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData; + + MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + } + + MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + m_islandAnalyticsData.clear(); + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + + void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) + { + m_solver = solver; + } + + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) + { + if (islandId < 0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_solverInfo->m_reportSolverAnalytics&1) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + } + else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i = 0; i < m_numConstraints; i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (; i < m_numConstraints; i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + for (i = 0; i < m_numMultiBodyConstraints; i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; + break; + } + } + //count the number of multi body constraints in this island + for (; i < m_numMultiBodyConstraints; i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + numCurMultiBodyConstraints++; + } + } + + //if (m_solverInfo->m_minimumSolverBatchSize<=1) + //{ + // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + //} else + { + for (i = 0; i < numBodies; i++) + { + bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY); + if (!isSoftBodyType) + { + m_bodies.push_back(bodies[i]); + } + else + { + m_softBodies.push_back(bodies[i]); + } + } + for (i = 0; i < numManifolds; i++) + m_manifolds.push_back(manifolds[i]); + for (i = 0; i < numCurConstraints; i++) + m_constraints.push_back(startConstraint[i]); + + for (i = 0; i < numCurMultiBodyConstraints; i++) + m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); + + if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(islandId); + } + else + { + //printf("deferred\n"); + } + } + } + } + + virtual void processConstraints(int islandId=-1) + { + 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; + + //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); + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + m_bodies.resize(0); + m_softBodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } +}; + + +#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */ diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h index 92d41dfac2..01d5583c2f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -111,6 +111,10 @@ struct btMultibodyLink 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. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. btVector3 m_appliedForce; // In WORLD frame btVector3 m_appliedTorque; // In WORLD frame @@ -119,6 +123,7 @@ struct btMultibodyLink btVector3 m_appliedConstraintTorque; // In WORLD frame btScalar m_jointPos[7]; + btScalar m_jointPos_interpolate[7]; //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. //It gets set to zero after each internal stepSimulation call @@ -152,6 +157,7 @@ struct btMultibodyLink m_parent(-1), m_zeroRotParentToThis(0, 0, 0, 1), m_cachedRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis_interpolate(0, 0, 0, 1), m_collider(0), m_flags(0), m_dofCount(0), @@ -174,6 +180,7 @@ struct btMultibodyLink m_dVector.setValue(0, 0, 0); m_eVector.setValue(0, 0, 0); m_cachedRVector.setValue(0, 0, 0); + m_cachedRVector_interpolate.setValue(0, 0, 0); m_appliedForce.setValue(0, 0, 0); m_appliedTorque.setValue(0, 0, 0); m_appliedConstraintForce.setValue(0, 0, 0); @@ -188,42 +195,43 @@ struct btMultibodyLink // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector = m_cachedRVector; switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + 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 - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector); + 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: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } @@ -233,7 +241,60 @@ struct btMultibodyLink btAssert(0); } } + m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis; + m_cachedRVector_interpolate = m_cachedRVector; } + + 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 |