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

multidof4 patch

Browse Source
This commit is contained in:
kubas
2014-01-09 00:26:24 +01:00
parent 75b8f7230d
commit 96ff69276f
12 changed files with 2661 additions and 428 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

File diff suppressed because it is too large Load Diff

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

@@ -45,21 +45,28 @@ public:
// initialization
//
btMultiBody(int n_links, // NOT including the base
btMultiBody(int n_links, // NOT including the base
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixed_base_, // whether the base is fixed (true) or can move (false)
bool can_sleep_);
bool fixedBase, // whether the base is fixed (true) or can move (false)
bool canSleep);
~btMultiBody();
btMultiBody(int n_links, // NOT including the base
int n_dofs, // NOT including 6 floating base dofs
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);
~btMultiBody();
void setupPrismatic(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal
int parent,
const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame.
const btVector3 &joint_axis, // in my frame
const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0.
const btQuaternion &rotParentToThis, // rotate points in parent frame to my frame.
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisComOffset, // vector from parent COM to my COM, in my frame, when q = 0.
bool disableParentCollision=false
);
@@ -67,20 +74,40 @@ public:
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0
const btVector3 &joint_axis, // in my frame
const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupSpherical(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
void setupPlanar(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &rotationAxis,
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
bool disableParentCollision=false);
#endif
const btMultibodyLink& getLink(int index) const
{
return links[index];
return m_links[index];
}
btMultibodyLink& getLink(int index)
{
return links[index];
return m_links[index];
}
@@ -106,12 +133,13 @@ public:
//
// get number of links, masses, moments of inertia
// get number of m_links, masses, moments of inertia
//
int getNumLinks() const { return links.size(); }
btScalar getBaseMass() const { return base_mass; }
const btVector3 & getBaseInertia() const { return base_inertia; }
int getNumLinks() const { return m_links.size(); }
int getNumDofs() const { return m_dofCount; }
btScalar getBaseMass() const { return m_baseMass; }
const btVector3 & getBaseInertia() const { return m_baseInertia; }
btScalar getLinkMass(int i) const;
const btVector3 & getLinkInertia(int i) const;
@@ -120,55 +148,60 @@ public:
// change mass (incomplete: can only change base mass and inertia at present)
//
void setBaseMass(btScalar mass) { base_mass = mass; }
void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
void setBaseMass(btScalar mass) { m_baseMass = mass; }
void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
//
// get/set pos/vel/rot/omega for the base link
//
const btVector3 & getBasePos() const { return base_pos; } // in world frame
const btVector3 & getBasePos() const { return m_basePos; } // in world frame
const btVector3 getBaseVel() const
{
return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]);
return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
} // in world frame
const btQuaternion & getWorldToBaseRot() const
{
return base_quat;
return m_baseQuat;
} // rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame
btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
base_pos = pos;
m_basePos = pos;
}
void setBaseVel(const btVector3 &vel)
{
m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2];
m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
}
void setWorldToBaseRot(const btQuaternion &rot)
{
base_quat = rot;
m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
}
void setBaseOmega(const btVector3 &omega)
{
m_real_buf[0]=omega[0];
m_real_buf[1]=omega[1];
m_real_buf[2]=omega[2];
m_realBuf[0]=omega[0];
m_realBuf[1]=omega[1];
m_realBuf[2]=omega[2];
}
//
// get/set pos/vel for child links (i = 0 to num_links-1)
// get/set pos/vel for child m_links (i = 0 to num_links-1)
//
btScalar getJointPos(int i) const;
btScalar getJointVel(int i) const;
btScalar * getJointVelMultiDof(int i);
btScalar * getJointPosMultiDof(int i);
void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot);
void setJointPosMultiDof(int i, btScalar *q);
void setJointVelMultiDof(int i, btScalar *qdot);
//
// direct access to velocities as a vector of 6 + num_links elements.
@@ -176,7 +209,7 @@ public:
//
const btScalar * getVelocityVector() const
{
return &m_real_buf[0];
return &m_realBuf[0];
}
/* btScalar * getVelocityVector()
{
@@ -185,7 +218,7 @@ public:
*/
//
// get the frames of reference (positions and orientations) of the child links
// get the frames of reference (positions and orientations) of the child m_links
// (i = 0 to num_links-1)
//
@@ -220,18 +253,21 @@ public:
void addBaseForce(const btVector3 &f)
{
base_force += f;
m_baseForce += f;
}
void addBaseTorque(const btVector3 &t) { base_torque += t; }
void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
void addLinkForce(int i, const btVector3 &f);
void addLinkTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
void addJointTorqueMultiDof(int i, int dof, btScalar Q);
void addJointTorqueMultiDof(int i, const btScalar *Q);
const btVector3 & getBaseForce() const { return base_force; }
const btVector3 & getBaseTorque() const { return base_torque; }
const btVector3 & getBaseForce() const { return m_baseForce; }
const btVector3 & getBaseTorque() const { return m_baseTorque; }
const btVector3 & getLinkForce(int i) const;
const btVector3 & getLinkTorque(int i) const;
btScalar getJointTorque(int i) const;
btScalar * getJointTorqueMultiDof(int i);
//
@@ -255,6 +291,11 @@ public:
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m);
void stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m);
// calcAccelerationDeltas
// input: force vector (in same format as jacobian, i.e.:
// 3 torque values, 3 force values, num_links joint torque values)
@@ -265,13 +306,17 @@ public:
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
// apply a delta-vee directly. used in sequential impulses code.
void applyDeltaVee(const btScalar * delta_vee)
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
m_real_buf[i] += delta_vee[i];
m_realBuf[i] += delta_vee[i];
}
}
@@ -300,12 +345,37 @@ public:
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
m_real_buf[i] += delta_vee[i] * multiplier;
m_realBuf[i] += delta_vee[i] * multiplier;
}
}
void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
{
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
// printf("%.4f ", delta_vee[dof]*multiplier);
//printf("\n");
btScalar sum = 0;
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
}
btScalar l = btSqrt(sum);
if (l>m_maxAppliedImpulse)
{
multiplier *= m_maxAppliedImpulse/l;
}
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_realBuf[dof] += delta_vee[dof] * multiplier;
}
}
// timestep the positions (given current velocities).
void stepPositions(btScalar dt);
void stepPositionsMultiDof(btScalar dt);
//
@@ -323,23 +393,31 @@ public:
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
void fillContactJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
//
// sleeping
//
void setCanSleep(bool canSleep)
{
can_sleep = canSleep;
m_canSleep = canSleep;
}
bool isAwake() const { return awake; }
bool isAwake() const { return m_awake; }
void wakeUp();
void goToSleep();
void checkMotionAndSleepIfRequired(btScalar timestep);
bool hasFixedBase() const
{
return fixed_base;
return m_fixedBase;
}
int getCompanionId() const
@@ -352,9 +430,9 @@ public:
m_companionId = id;
}
void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links
void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
{
links.resize(numLinks);
m_links.resize(numLinks);
}
btScalar getLinearDamping() const
@@ -369,6 +447,10 @@ public:
{
return m_angularDamping;
}
void setAngularDamping( btScalar damp)
{
m_angularDamping = damp;
}
bool getUseGyroTerm() const
{
@@ -396,6 +478,9 @@ public:
return m_hasSelfCollision;
}
bool isMultiDof() { return m_isMultiDof; }
void forceMultiDof();
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@@ -403,57 +488,64 @@ private:
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
#endif
void updateLinksDofOffsets() { int dofOffset = 0; for(int bidx = 0; bidx < m_links.size(); ++bidx) { m_links[bidx].m_dofOffset = dofOffset; dofOffset += m_links[bidx].m_dofCount; } }
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
private:
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
btVector3 base_pos; // position of COM of base (world frame)
btQuaternion base_quat; // rotates world points into base frame
btVector3 m_basePos; // position of COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
btScalar base_mass; // mass of the base
btVector3 base_inertia; // inertia of the base (in local frame; diagonal)
btScalar m_baseMass; // mass of the base
btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
btVector3 base_force; // external force applied to base. World frame.
btVector3 base_torque; // external torque applied to base. World frame.
btVector3 m_baseForce; // external force applied to base. World frame.
btVector3 m_baseTorque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
int m_dofCount;
//
// real_buf:
// realBuf:
// offset size array
// 0 6 + num_links v (base_omega; base_vel; joint_vels)
// 6+num_links num_links D
//
// vector_buf:
// vectorBuf:
// offset size array
// 0 num_links h_top
// num_links num_links h_bottom
//
// matrix_buf:
// matrixBuf:
// offset size array
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> m_real_buf;
btAlignedObjectArray<btVector3> vector_buf;
btAlignedObjectArray<btMatrix3x3> matrix_buf;
btAlignedObjectArray<btScalar> m_realBuf;
btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 cached_inertia_top_left;
btMatrix3x3 cached_inertia_top_right;
btMatrix3x3 cached_inertia_lower_left;
btMatrix3x3 cached_inertia_lower_right;
btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight;
btMatrix3x3 m_cachedInertiaLowerLeft;
btMatrix3x3 m_cachedInertiaLowerRight;
bool fixed_base;
bool m_fixedBase;
// Sleep parameters.
bool awake;
bool can_sleep;
btScalar sleep_timer;
bool m_awake;
bool m_canSleep;
btScalar m_sleepTimer;
int m_companionId;
btScalar m_linearDamping;
@@ -461,6 +553,7 @@ private:
bool m_useGyroTerm;
btScalar m_maxAppliedImpulse;
bool m_hasSelfCollision;
bool m_isMultiDof;
};
#endif

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

@@ -6,14 +6,31 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows),
m_numRows(numRows),
m_isUnilateral(isUnilateral),
m_maxAppliedImpulse(100)
m_maxAppliedImpulse(100),
m_jacSizeA(0),
m_jacSizeBoth(0)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
if(bodyA)
{
if(bodyA->isMultiDof())
m_jacSizeA = (6 + bodyA->getNumDofs());
else
m_jacSizeA = (6 + bodyA->getNumLinks());
}
if(bodyB)
{
if(bodyB->isMultiDof())
m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
else
m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
}
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
@@ -42,15 +59,15 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; //total dof count of tree A
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0)
if (constraintRow.m_deltaVelAindex <0) //if this multibody does not have a place allocated in m_deltaVelocities...
{
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); //=> each constrained tree's dofs are represented in m_deltaVelocities
} else
{
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
@@ -58,18 +75,21 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
for (int i=0;i<ndofA;i++)
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
if(multiBodyA->isMultiDof())
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
else
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
@@ -87,7 +107,10 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
if(multiBodyB->isMultiDof())
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
}
{
@@ -101,7 +124,7 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
@@ -113,7 +136,7 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
@@ -161,14 +184,14 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -257,7 +280,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -277,9 +300,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
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);
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
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);
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
@@ -290,7 +319,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -306,8 +335,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
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);
if(multiBodyB->isMultiDof())
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
@@ -328,7 +363,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
@@ -347,7 +382,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
@@ -404,7 +439,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -417,7 +452,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -493,15 +528,20 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
//commented out on purpose, see below
//if (penetration>0)
//{
// positionalError = 0;
// velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
//} else
//{
// positionalError = -penetration * erp/infoGlobal.m_timeStep;
//}
//we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints)
//both are valid in general and definitely so in the case of a point2Point constraint
positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;

26
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -28,8 +28,8 @@ struct btSolverInfo;
struct btMultiBodyJacobianData
{
btAlignedObjectArray<btScalar> m_jacobians;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
btAlignedObjectArray<btScalar> m_deltaVelocities;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
@@ -48,10 +48,10 @@ protected:
int m_linkA;
int m_linkB;
int m_num_rows;
int m_jac_size_A;
int m_jac_size_both;
int m_pos_offset;
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
int m_posOffset;
bool m_isUnilateral;
@@ -99,7 +99,7 @@ public:
int getNumRows() const
{
return m_num_rows;
return m_numRows;
}
btMultiBody* getMultiBodyA()
@@ -116,12 +116,12 @@ public:
// NOTE: ignored position for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_pos_offset + row];
return m_data[m_posOffset + row];
}
void setPosition(int row, btScalar pos)
{
m_data[m_pos_offset + row] = pos;
m_data[m_posOffset + row] = pos;
}
@@ -135,19 +135,19 @@ public:
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
btScalar* jacobianA(int row)
{
return &m_data[m_num_rows + row * m_jac_size_both];
return &m_data[m_numRows + row * m_jacSizeBoth];
}
const btScalar* jacobianA(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both)];
return &m_data[m_numRows + (row * m_jacSizeBoth)];
}
btScalar* jacobianB(int row)
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
const btScalar* jacobianB(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
btScalar getMaxAppliedImpulse() const

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

@@ -108,10 +108,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 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
} else if(c.m_solverBodyIdA >= 0)
{
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -119,10 +119,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumLinks() + 6;
ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 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
} else if(c.m_solverBodyIdB >= 0)
{
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
@@ -151,8 +151,11 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
} else
if(c.m_multiBodyA->isMultiDof())
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
else
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
} else if(c.m_solverBodyIdA >= 0)
{
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
@@ -160,8 +163,11 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
} else
if(c.m_multiBodyB->isMultiDof())
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
else
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
} else if(c.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
@@ -180,14 +186,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 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->getNumLinks() + 6;
ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 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];
}
@@ -257,7 +263,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -277,9 +283,15 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
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];
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
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);
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
@@ -290,7 +302,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -306,8 +318,14 @@ 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());
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);
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
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);
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
@@ -328,7 +346,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
@@ -347,7 +365,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
@@ -404,7 +422,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -417,7 +435,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -452,7 +470,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
if(multiBodyA->isMultiDof())
multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
else
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
@@ -463,7 +484,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
if(multiBodyB->isMultiDof())
multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
else
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{

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

@@ -451,7 +451,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
scratch_r.resize(bod->getNumLinks()+1);
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
@@ -463,7 +464,10 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
if(bod->isMultiDof())
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
else
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
}
}
@@ -503,13 +507,14 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
int nLinks = bod->getNumLinks();
///base + num links
///base + num m_links
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
bod->stepPositions(timeStep);
if(bod->isMultiDof())
bod->stepPositionsMultiDof(timeStep);
else
bod->stepPositions(timeStep);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();

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

@@ -22,6 +22,7 @@ subject to the following restrictions:
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
//:btMultiBodyConstraint(body,0,link,-1,2,true),
:btMultiBodyConstraint(body,body,link,link,2,true),
m_lowerBound(lower),
m_upperBound(upper)
@@ -32,11 +33,14 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
// row 1: the upper bound
jacobianB(1)[6 + link] = -1;
// row 0: the lower bound
jacobianA(0)[offset] = 1;
// row 1: the upper bound
//jacobianA(1)[offset] = -1;
jacobianB(1)[offset] = -1;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
@@ -44,28 +48,34 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if(m_bodyA)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if(m_bodyB)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
@@ -79,7 +89,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
// directions were set in the ctor and never change.
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
@@ -90,7 +100,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,m_maxAppliedImpulse);
{
btScalar penetration = getPosition(row);
btScalar positionalError = 0.f;

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

@@ -22,6 +22,7 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,link,1,true),
m_desiredVelocity(desiredVelocity)
{
@@ -32,8 +33,11 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{

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

@@ -24,6 +24,303 @@ enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
// Various spatial helper functions
//
namespace {
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
struct btSpatialForceVector
{
btVector3 m_topVec, m_bottomVec;
//
btSpatialForceVector() { setZero(); }
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
setValue(ax, ay, az, lx, ly, lz);
}
//
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
}
//
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;
}
//
const btVector3 & getLinear() const { return m_topVec; }
const btVector3 & getAngular() const { return m_bottomVec; }
//
void setLinear(const btVector3 &linear) { m_topVec = linear; }
void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
//
void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
void addLinear(const btVector3 &linear) { m_topVec += linear; }
//
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
//
btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
//btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
};
struct btSpatialMotionVector
{
btVector3 m_topVec, m_bottomVec;
//
btSpatialMotionVector() { setZero(); }
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
//
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
}
//
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;
}
//
const btVector3 & getAngular() const { return m_topVec; }
const btVector3 & getLinear() const { return m_bottomVec; }
//
void setAngular(const btVector3 &angular) { m_topVec = angular; }
void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
//
void addAngular(const btVector3 &angular) { m_topVec += angular; }
void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
//
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
//
btScalar dot(const btSpatialForceVector &b) const
{
return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
}
//
template<typename SpatialVectorType>
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
{
out.m_topVec = m_topVec.cross(b.m_topVec);
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
}
template<typename SpatialVectorType>
SpatialVectorType cross(const SpatialVectorType &b) const
{
SpatialVectorType out;
out.m_topVec = m_topVec.cross(b.m_topVec);
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
return out;
}
//
btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
};
struct btSymmetricSpatialDyad
{
btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
//
btSymmetricSpatialDyad() { setIdentity(); }
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
//
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
{
m_topLeftMat = topLeftMat;
m_topRightMat = topRightMat;
m_bottomLeftMat = bottomLeftMat;
}
//
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
{
m_topLeftMat += topLeftMat;
m_topRightMat += topRightMat;
m_bottomLeftMat += bottomLeftMat;
}
//
void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); }
//
btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
{
m_topLeftMat -= mat.m_topLeftMat;
m_topRightMat -= mat.m_topRightMat;
m_bottomLeftMat -= mat.m_bottomLeftMat;
return *this;
}
//
btSpatialForceVector operator * (const btSpatialMotionVector &vec)
{
return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
}
};
struct btSpatialTransformationMatrix
{
btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
btVector3 m_trnVec;
//
enum eOutputOperation
{
None = 0,
Add = 1,
Subtract = 2
};
//
template<typename SpatialVectorType>
void transform( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
{
outVec.m_topVec = m_rotMat * inVec.m_topVec;
outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Add)
{
outVec.m_topVec += m_rotMat * inVec.m_topVec;
outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Subtract)
{
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
}
}
template<typename SpatialVectorType>
void transformRotationOnly( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
{
outVec.m_topVec = m_rotMat * inVec.m_topVec;
outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Add)
{
outVec.m_topVec += m_rotMat * inVec.m_topVec;
outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Subtract)
{
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
}
}
template<typename SpatialVectorType>
void transformInverse( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
{
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
}
else if(outOp == Add)
{
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
}
else if(outOp == Subtract)
{
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
}
}
void transformInverse( const btSymmetricSpatialDyad &inMat,
btSymmetricSpatialDyad &outMat,
eOutputOperation outOp = None)
{
const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1],
m_trnVec[2], 0, -m_trnVec[0],
-m_trnVec[1], m_trnVec[0], 0);
if(outOp == None)
{
outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
}
else if(outOp == Add)
{
outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
}
else if(outOp == Subtract)
{
outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
}
}
template<typename SpatialVectorType>
SpatialVectorType operator * (const SpatialVectorType &vec)
{
SpatialVectorType out;
transform(vec, out);
return out;
}
};
template<typename SpatialVectorType>
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
{
//output op maybe?
out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
//maybe simple a*spatTranspose(a) would be nicer?
}
template<typename SpatialVectorType>
btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
{
btSymmetricSpatialDyad out;
out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
return out;
//maybe simple a*spatTranspose(a) would be nicer?
}
#endif
}
//
// Link struct
//
@@ -33,75 +330,155 @@ struct btMultibodyLink
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar joint_pos; // qi
btScalar m_mass; // mass of link
btVector3 m_inertia; // inertia of link (local frame; diagonal)
btScalar mass; // mass of link
btVector3 inertia; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: axis_top = zero;
// axis_bottom = unit vector along the joint axis.
// for revolute: axis_top = unit vector along the rotation axis (u);
// axis_bottom = u cross d_vector.
btVector3 axis_top;
btVector3 axis_bottom;
btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
// e_vector is constant, but depends on the joint type
// m_eVector is constant, but depends on the joint type
// prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
btVector3 e_vector;
btVector3 m_eVector;
bool is_revolute; // true = revolute, false = prismatic
enum eFeatherstoneJointType
{
eRevolute = 0,
ePrismatic = 1,
eSpherical = 2,
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
ePlanar = 3,
#endif
eInvalid
};
btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame
btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame.
eFeatherstoneJointType m_jointType;
int m_dofCount, m_posVarCount; //redundant but handy
btVector3 applied_force; // In WORLD frame
btVector3 applied_torque; // In WORLD frame
btScalar joint_torque;
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: m_axesTop[0] = zero;
// m_axesBottom[0] = unit vector along the joint axis.
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
//
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
//
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
// m_axesTop[1][2] = zero
// m_axesBottom[0] = zero
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
btVector3 m_axesTop[6];
btVector3 m_axesBottom[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); }
const btVector3 & getAxisTop(int dof) const { return m_axesTop[dof]; }
const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; }
#else
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
#endif
int m_dofOffset;
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.
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btScalar m_jointPos[7];
btScalar m_jointTorque[6]; //TODO
class btMultiBodyLinkCollider* m_collider;
int m_flags;
// ctor: set some sensible defaults
btMultibodyLink()
: joint_pos(0),
mass(1),
parent(-1),
zero_rot_parent_to_this(1, 0, 0, 0),
is_revolute(false),
cached_rot_parent_to_this(1, 0, 0, 0),
joint_torque(0),
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(1, 0, 0, 0),
m_cachedRotParentToThis(1, 0, 0, 0),
m_collider(0),
m_flags(0)
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid)
{
inertia.setValue(1, 1, 1);
axis_top.setValue(0, 0, 0);
axis_bottom.setValue(1, 0, 0);
d_vector.setValue(0, 0, 0);
e_vector.setValue(0, 0, 0);
cached_r_vector.setValue(0, 0, 0);
applied_force.setValue( 0, 0, 0);
applied_torque.setValue(0, 0, 0);
m_inertia.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
m_eVector.setValue(0, 0, 0);
m_cachedRVector.setValue(0, 0, 0);
m_appliedForce.setValue( 0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
//
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
m_jointPos[3] = 1.f; //"quat.w"
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
}
// routine to update cached_rot_parent_to_this and cached_r_vector
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCache()
{
if (is_revolute)
//multidof
if (m_jointType == eRevolute)
{
cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
} else
{
// cached_rot_parent_to_this never changes, so no need to update
cached_r_vector = e_vector + joint_pos * axis_bottom;
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
}
}
void updateCacheMultiDof()
{
switch(m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(m_jointPos[0], m_jointPos[1], m_jointPos[2], -m_jointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-m_jointPos[0]), m_jointPos[1] * m_axesBottom[1] + m_jointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
#endif
}
}
};

4
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
View File

@@ -74,14 +74,14 @@ public:
if (m_link>=0)
{
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link)
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
return false;
}
if (other->m_link>=0)
{
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link)
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
return false;
}
return true;

2
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "BulletDynamics/Dynamics/btRigidBody.h"
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,3,false),
:btMultiBodyConstraint(body,0,link,-1,3,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),

2
src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
View File

@@ -28,6 +28,8 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1)
{}
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
btVector3 m_relpos1CrossNormal;