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

Only support btMultiBody multi-dof version (remove non-multi-dof path)

Browse Source 
Use ATTRIBUTE_ALIGNED16 for btMultiBody
Always disable parentCollision for btMultiBody::setupFixed
This commit is contained in:
erwincoumans
2015-11-05 21:17:46 -08:00
parent d6464ce40d
commit 2920d7e61f
16 changed files with 125 additions and 374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -15,7 +15,7 @@ public:
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) =0;
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) =0;
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;

4
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -18,12 +18,12 @@ m_guiHelper(guiHelper)
}
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
{
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
m_mb2urdfLink.resize(totalNumJoints+1,-2);
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
return m_bulletMultiBody;
}

2
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -43,7 +43,7 @@ public:
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);

6
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -241,11 +241,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
if (cache.m_bulletMultiBody==0)
{
bool multiDof = true;
bool canSleep = false;
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1;
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
}
@@ -271,7 +271,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
//b3Printf("Fixed joint (btMultiBody)\n");
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin());
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);

11
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@@ -136,8 +136,8 @@ void InvertedPendulumPDControl::initPhysics()
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -201,7 +201,7 @@ void InvertedPendulumPDControl::initPhysics()
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -249,10 +249,9 @@ void InvertedPendulumPDControl::initPhysics()
{
btScalar q0 = 180.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
{
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

11
examples/MultiBody/MultiBodyConstraintFeedback.cpp
View File

@@ -131,8 +131,8 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -196,7 +196,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -244,10 +244,9 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
{
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

9
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -161,10 +161,9 @@ void MultiDofDemo::initPhysics()
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
{
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
@@ -260,8 +259,8 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
}
bool canSleep = false;
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);

11
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -135,8 +135,8 @@ void TestJointTorqueSetup::initPhysics()
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -200,7 +200,7 @@ void TestJointTorqueSetup::initPhysics()
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -248,10 +248,9 @@ void TestJointTorqueSetup::initPhysics()
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
{
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

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

@@ -91,8 +91,7 @@ btMultiBody::btMultiBody(int n_links,
btScalar mass,
const btVector3 &inertia,
bool fixedBase,
bool canSleep,
bool multiDof)
bool canSleep)
:
m_baseCollider(0),
m_baseName(0),
@@ -112,7 +111,6 @@ btMultiBody::btMultiBody(int n_links,
m_maxAppliedImpulse(1000.f),
m_maxCoordinateVelocity(100.f),
m_hasSelfCollision(true),
m_isMultiDof(multiDof),
__posUpdated(false),
m_dofCount(0),
m_posVarCnt(0),
@@ -120,14 +118,6 @@ btMultiBody::btMultiBody(int n_links,
m_useGlobalVelocities(false),
m_internalNeedsJointFeedback(false)
{
if(!m_isMultiDof)
{
m_vectorBuf.resize(2*n_links);
m_realBuf.resize(6 + 2*n_links);
m_posVarCnt = n_links;
}
m_links.resize(n_links);
m_matrixBuf.resize(n_links + 1);
@@ -146,10 +136,8 @@ void btMultiBody::setupFixed(int i,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
const btVector3 &thisPivotToThisComOffset)
{
btAssert(m_isMultiDof);
m_links[i].m_mass = mass;
m_links[i].m_inertiaLocal = inertia;
@@ -162,15 +150,10 @@ void btMultiBody::setupFixed(int i,
m_links[i].m_dofCount = 0;
m_links[i].m_posVarCount = 0;
if (disableParentCollision)
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
m_links[i].updateCacheMultiDof();
//
//if(m_isMultiDof)
// resizeInternalMultiDofBuffers();
//
updateLinksDofOffsets();
}
@@ -186,11 +169,8 @@ void btMultiBody::setupPrismatic(int i,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
if(m_isMultiDof)
{
m_dofCount += 1;
m_posVarCnt += 1;
}
m_links[i].m_mass = mass;
m_links[i].m_inertiaLocal = inertia;
@@ -211,12 +191,9 @@ void btMultiBody::setupPrismatic(int i,
if (disableParentCollision)
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
if(m_isMultiDof)
m_links[i].updateCacheMultiDof();
else
m_links[i].updateCache();
//
if(m_isMultiDof)
updateLinksDofOffsets();
}
@@ -230,11 +207,8 @@ void btMultiBody::setupRevolute(int i,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
if(m_isMultiDof)
{
m_dofCount += 1;
m_posVarCnt += 1;
}
m_links[i].m_mass = mass;
m_links[i].m_inertiaLocal = inertia;
@@ -254,12 +228,8 @@ void btMultiBody::setupRevolute(int i,
if (disableParentCollision)
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
if(m_isMultiDof)
m_links[i].updateCacheMultiDof();
else
m_links[i].updateCache();
//
if(m_isMultiDof)
updateLinksDofOffsets();
}
@@ -274,7 +244,7 @@ void btMultiBody::setupSpherical(int i,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_dofCount += 3;
m_posVarCnt += 4;
@@ -315,7 +285,7 @@ void btMultiBody::setupPlanar(int i,
const btVector3 &parentComToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_dofCount += 3;
m_posVarCnt += 3;
@@ -357,7 +327,6 @@ void btMultiBody::setupPlanar(int i,
void btMultiBody::finalizeMultiDof()
{
btAssert(m_isMultiDof);
m_deltaV.resize(0);
m_deltaV.resize(6 + m_dofCount);
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
@@ -388,7 +357,7 @@ btScalar btMultiBody::getJointPos(int i) const
btScalar btMultiBody::getJointVel(int i) const
{
return m_realBuf[6 + i];
return m_realBuf[6 + m_links[i].m_dofOffset];
}
btScalar * btMultiBody::getJointPosMultiDof(int i)
@@ -415,7 +384,7 @@ const btScalar * btMultiBody::getJointVelMultiDof(int i) const
void btMultiBody::setJointPos(int i, btScalar q)
{
m_links[i].m_jointPos[0] = q;
m_links[i].updateCache();
m_links[i].updateCacheMultiDof();
}
void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
@@ -428,7 +397,7 @@ void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
void btMultiBody::setJointVel(int i, btScalar qdot)
{
m_realBuf[6 + i] = qdot;
m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
}
void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
@@ -1939,41 +1908,6 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
//int dummy = 0;
}
void btMultiBody::stepPositions(btScalar dt)
{
int num_links = getNumLinks();
// step position by adding dt * velocity
btVector3 v = getBaseVel();
m_basePos += dt * v;
// "exponential map" method for the rotation
btVector3 base_omega = getBaseOmega();
const btScalar omega_norm = base_omega.norm();
const btScalar omega_times_dt = omega_norm * dt;
const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
{
const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2
const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega|
const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|)
m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
} else
{
m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
}
// Make sure the quaternion represents a valid rotation.
// (Not strictly necessary, but helps prevent any round-off errors from building up.)
m_baseQuat.normalize();
// Finally we can update m_jointPos for each of the m_links
for (int i = 0; i < num_links; ++i)
{
float jointVel = getJointVel(i);
m_links[i].m_jointPos[0] += dt * jointVel;
m_links[i].updateCache();
}
}
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
{
@@ -2238,98 +2172,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
}
}
void btMultiBody::fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
// temporary space
int num_links = getNumLinks();
scratch_v.resize(2*num_links + 2);
scratch_m.resize(num_links + 1);
btVector3 * v_ptr = &scratch_v[0];
btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(num_links);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
const btVector3 p_minus_com_world = contact_point - m_basePos;
rot_from_world[0] = btMatrix3x3(m_baseQuat);
p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
n_local[0] = rot_from_world[0] * normal;
// omega coeffients first.
if (this->m_fixedBase)
{
for (int i=0;i<6;i++)
{
jac[i]=0;
}
} else
{
btVector3 omega_coeffs;
omega_coeffs = p_minus_com_world.cross(normal);
jac[0] = omega_coeffs[0];
jac[1] = omega_coeffs[1];
jac[2] = omega_coeffs[2];
// then v coefficients
jac[3] = normal[0];
jac[4] = normal[1];
jac[5] = normal[2];
}
// Set remaining jac values to zero for now.
for (int i = 6; i < 6 + num_links; ++i) {
jac[i] = 0;
}
// Qdot coefficients, if necessary.
if (num_links > 0 && link > -1) {
// TODO: speed this up -- don't calculate for m_links we don't need.
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
// which is resulting in repeated work being done...)
// calculate required normals & positions in the local frames.
for (int i = 0; i < num_links; ++i) {
// transform to local frame
const int parent = m_links[i].m_parent;
const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
n_local[i+1] = mtx * n_local[parent+1];
p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
// calculate the jacobian entry
if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
} else {
results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
}
}
// Now copy through to output.
//printf("jac[%d] = ", link);
while (link != -1)
{
jac[6 + link] = results[link];
//printf("%.2f\t", jac[6 + link]);
link = m_links[link].m_parent;
}
//printf("]\n");
}
}
void btMultiBody::wakeUp()
{
@@ -2354,16 +2196,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
btScalar motion = 0;
if(m_isMultiDof)
{
for (int i = 0; i < 6 + m_dofCount; ++i)
motion += m_realBuf[i] * m_realBuf[i];
}
else
{
for (int i = 0; i < 6 + num_links; ++i)
motion += m_realBuf[i] * m_realBuf[i];
}
if (motion < SLEEP_EPSILON) {

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

@@ -45,7 +45,7 @@
#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;
class btMultiBody
ATTRIBUTE_ALIGNED16(class) btMultiBody
{
public:
@@ -60,21 +60,19 @@ public:
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixedBase, // whether the base is fixed (true) or can move (false)
bool canSleep,
bool multiDof = false
);
bool canSleep);
virtual ~btMultiBody();
//note: fixed link collision with parent is always disabled
void setupFixed(int linkIndex,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);
const btVector3 &thisPivotToThisComOffset);
void setupPrismatic(int i,
@@ -447,7 +445,6 @@ void addJointTorque(int i, btScalar Q);
// timestep the positions (given current velocities).
void stepPositions(btScalar dt);
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
@@ -458,15 +455,7 @@ void addJointTorque(int i, btScalar Q);
// This routine fills out a contact constraint jacobian for this body.
// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
// 'normal' & 'contact_point' are both given in world coordinates.
void fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
//multidof version of fillContactJacobian
void fillContactJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal,
@@ -576,7 +565,7 @@ void addJointTorque(int i, btScalar Q);
return m_hasSelfCollision;
}
bool isMultiDof() { return m_isMultiDof; }
void finalizeMultiDof();
void useRK4Integration(bool use) { m_useRK4 = use; }
@@ -700,7 +689,7 @@ private:
btScalar m_maxAppliedImpulse;
btScalar m_maxCoordinateVelocity;
bool m_hasSelfCollision;
bool m_isMultiDof;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;

30
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -23,18 +23,12 @@ void btMultiBodyConstraint::updateJacobianSizes()
{
if(m_bodyA)
{
if(m_bodyA->isMultiDof())
m_jacSizeA = (6 + m_bodyA->getNumDofs());
else
m_jacSizeA = (6 + m_bodyA->getNumLinks());
}
if(m_bodyB)
{
if(m_bodyB->isMultiDof())
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
else
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
}
else
m_jacSizeBoth = m_jacSizeA;
@@ -101,7 +95,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -128,10 +122,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
else
{
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
else
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
@@ -140,10 +131,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
//determine..
if(multiBodyA->isMultiDof())
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
else
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
@@ -167,7 +155,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -189,10 +177,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
else
{
if(multiBodyB->isMultiDof())
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
else
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
@@ -201,10 +186,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
//determine..
if(multiBodyB->isMultiDof())
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
@@ -231,7 +213,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
@@ -249,7 +231,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
//
if (multiBodyB)
{
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
@@ -290,7 +272,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -301,7 +283,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
if (multiBodyB)
{
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];

51
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -123,7 +123,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyA)
{
ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
ndofA = c.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
} else if(c.m_solverBodyIdA >= 0)
@@ -134,7 +134,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyB)
{
ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
ndofB = c.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
} else if(c.m_solverBodyIdB >= 0)
@@ -169,10 +169,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
if(c.m_multiBodyA->isMultiDof())
c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
else
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdA >= 0)
{
@@ -185,10 +182,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
if(c.m_multiBodyB->isMultiDof())
c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
else
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdB >= 0)
{
@@ -209,14 +203,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
if (c.m_multiBodyA)
{
ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
ndofA = c.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
}
if (c.m_multiBodyB)
{
ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
ndofB = c.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
}
@@ -296,7 +290,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -316,15 +310,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
else
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
else
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
@@ -349,7 +337,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -365,14 +353,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
if(multiBodyB->isMultiDof())
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
else
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
if(multiBodyB->isMultiDof())
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
@@ -399,7 +381,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
int ndofA = 0;
if (multiBodyA)
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
@@ -418,7 +400,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
if (multiBodyB)
{
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
@@ -467,7 +449,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -480,7 +462,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
if (multiBodyB)
{
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -518,10 +500,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
else
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
@@ -532,10 +512,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
if(multiBodyB->isMultiDof())
multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
else
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
@@ -904,8 +881,6 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (c.m_multiBodyA)
{
if(c.m_multiBodyA->isMultiDof())
{
c.m_multiBodyA->setCompanionId(-1);
btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
@@ -920,11 +895,8 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
}
}
}
if (c.m_multiBodyB)
{
if(c.m_multiBodyB->isMultiDof())
{
{
c.m_multiBodyB->setCompanionId(-1);
@@ -945,7 +917,6 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
}
}
}
}
#endif
#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS

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

@@ -505,7 +505,6 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
if(bod->isMultiDof())
{
if(!bod->isUsingRK4Integration())
{
@@ -667,10 +666,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}
}
else//if(bod->isMultiDof())
{
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
@@ -709,7 +705,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
if(bod->isMultiDof())
{
if(!bod->isUsingRK4Integration())
{
@@ -760,7 +756,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
///base + num m_links
if(bod->isMultiDof())
{
if(!bod->isPosUpdated())
bod->stepPositionsMultiDof(timeStep);
@@ -773,10 +769,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
bod->setPosUpdated(false);
}
}
else
{
bod->stepPositions(timeStep);
}
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);

3
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@@ -38,7 +38,7 @@ void btMultiBodyJointLimitConstraint::finalizeMultiDof()
allocateJacobiansMultiDof();
unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
// row 0: the lower bound
jacobianA(0)[offset] = 1;
@@ -124,7 +124,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
if (m_bodyA->isMultiDof())
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));

3
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -39,7 +39,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA);
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
// row 0: the lower bound
// row 0: the lower bound
@@ -118,7 +118,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
if (m_bodyA->isMultiDof())
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));

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

@@ -165,20 +165,6 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
}
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCache()
{
//multidof
if (m_jointType == eRevolute)
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
} else
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
}
}
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);