Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #2351 from xhan0619/Deformable

Browse Source 
Add deformable body world and solver
This commit is contained in:
erwincoumans
2019-08-14 21:13:41 -07:00
committed by GitHub
parent 75df77611a 69a02302aa
commit 7fa70c3857
42 changed files with 5120 additions and 475 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -17,7 +17,6 @@ subject to the following restrictions:
#define BT_DISCRETE_DYNAMICS_WORLD_H
#include "btDynamicsWorld.h"
class btDispatcher;
class btOverlappingPairCache;
class btConstraintSolver;
@@ -26,6 +25,7 @@ class btTypedConstraint;
class btActionInterface;
class btPersistentManifold;
class btIDebugDraw;
struct InplaceSolverIslandCallback;
#include "LinearMath/btAlignedObjectArray.h"
@@ -76,7 +76,7 @@ protected:
virtual void calculateSimulationIslands();
virtual void solveConstraints(btContactSolverInfo & solverInfo);
virtual void updateActivationState(btScalar timeStep);
@@ -95,7 +95,7 @@ protected:
void serializeRigidBodies(btSerializer * serializer);
void serializeDynamicsWorldInfo(btSerializer * serializer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -107,6 +107,8 @@ public:
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
virtual void solveConstraints(btContactSolverInfo & solverInfo);
virtual void synchronizeMotionStates();
///this can be useful to synchronize a single rigid body -> graphics object
@@ -227,6 +229,16 @@ public:
{
return m_latencyMotionStateInterpolation;
}
btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
{
return m_nonStaticRigidBodies;
}
const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
{
return m_nonStaticRigidBodies;
}
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

3
src/BulletDynamics/Dynamics/btDynamicsWorld.h
View File

@@ -34,7 +34,8 @@ enum btDynamicsWorldType
BT_CONTINUOUS_DYNAMICS_WORLD = 3,
BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
BT_GPU_DYNAMICS_WORLD = 5,
BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6
BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.

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

@@ -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 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)
{
@@ -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;
@@ -2006,6 +2174,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
}
}
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);

23
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -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)
{
@@ -273,6 +286,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 +436,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 +599,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 +683,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)

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

@@ -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");
@@ -421,292 +427,19 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
solveExternalForces(solverInfo);
buildIslands();
solveInternalConstraints(solverInfo);
}
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)
}
}
#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;
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;
#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);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
} //if (!isSleeping)
}
}
void btMultiBodyDynamicsWorld::buildIslands()
{
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
}
void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
{
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
@@ -760,11 +493,306 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}
}
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)
}
}
#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;
bool isConstraintPass = false;
{
if (!bod->isUsingRK4Integration())
{
const btScalar linearDamp = bod->getLinearDamping();
// const btScalar angularDamp = bod->getAngularDamping();
bod->setLinearDamping(0);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
bod->setLinearDamping(linearDamp);
// bod->setAngularDamping(angularDamp);
}
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);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
} //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 +815,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)

14
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -47,7 +47,7 @@ protected:
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void serializeMultiBodies(btSerializer* serializer);
@@ -55,7 +55,8 @@ 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 +96,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 +114,10 @@ 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

79
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -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
@@ -188,42 +193,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;
}
@@ -234,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