mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
Merge branch 'master' of http://github.com/kalesony/bullet3
This commit is contained in:
Erwin Coumans
commit
a8e60b4ee5
@ -408,7 +408,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
|
||||
{
|
||||
if (1)
|
||||
{
|
||||
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000);
|
||||
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,0,500000);
|
||||
world->addMultiBodyConstraint(con);
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -45,21 +45,23 @@ 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,
|
||||
bool multiDof = false
|
||||
);
|
||||
|
||||
~btMultiBody();
|
||||
~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 +69,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 +128,14 @@ 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; }
|
||||
int getNumPosVars() const { return m_posVarCnt; }
|
||||
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 +144,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 +205,7 @@ public:
|
||||
//
|
||||
const btScalar * getVelocityVector() const
|
||||
{
|
||||
return &m_real_buf[0];
|
||||
return &m_realBuf[0];
|
||||
}
|
||||
/* btScalar * getVelocityVector()
|
||||
{
|
||||
@ -185,7 +214,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 +249,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 +287,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 +302,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 +341,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, btScalar *pq = 0, btScalar *pqd = 0);
|
||||
|
||||
|
||||
//
|
||||
@ -323,23 +389,43 @@ public:
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
|
||||
|
||||
//multidof version of fillContactJacobian
|
||||
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 { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
|
||||
|
||||
//a more general version of fillContactJacobianMultiDof which does not assume..
|
||||
//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
|
||||
void filConstraintJacobianMultiDof(int link,
|
||||
const btVector3 &contact_point,
|
||||
const btVector3 &normal_ang,
|
||||
const btVector3 &normal_lin,
|
||||
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 +438,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 +455,10 @@ public:
|
||||
{
|
||||
return m_angularDamping;
|
||||
}
|
||||
void setAngularDamping( btScalar damp)
|
||||
{
|
||||
m_angularDamping = damp;
|
||||
}
|
||||
|
||||
bool getUseGyroTerm() const
|
||||
{
|
||||
@ -396,6 +486,16 @@ public:
|
||||
return m_hasSelfCollision;
|
||||
}
|
||||
|
||||
bool isMultiDof() { return m_isMultiDof; }
|
||||
void finalizeMultiDof();
|
||||
|
||||
void useRK4Integration(bool use) { m_useRK4 = use; }
|
||||
bool isUsingRK4Integration() const { return m_useRK4; }
|
||||
void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
|
||||
bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
|
||||
|
||||
bool __posUpdated;
|
||||
|
||||
private:
|
||||
btMultiBody(const btMultiBody &); // not implemented
|
||||
void operator=(const btMultiBody &); // not implemented
|
||||
@ -403,57 +503,74 @@ 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, cfgOffset = 0;
|
||||
for(int bidx = 0; bidx < m_links.size(); ++bidx)
|
||||
{
|
||||
m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
|
||||
dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
|
||||
}
|
||||
}
|
||||
|
||||
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, m_posVarCnt;
|
||||
|
||||
bool m_useRK4, m_useGlobalVelocities;
|
||||
|
||||
//
|
||||
// real_buf:
|
||||
// realBuf:
|
||||
// offset size array
|
||||
// 0 6 + num_links v (base_omega; base_vel; joint_vels)
|
||||
// 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
|
||||
// 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 +578,7 @@ private:
|
||||
bool m_useGyroTerm;
|
||||
btScalar m_maxAppliedImpulse;
|
||||
bool m_hasSelfCollision;
|
||||
bool m_isMultiDof;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,263 +1,85 @@
|
||||
#include "btMultiBodyConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
|
||||
|
||||
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
|
||||
:m_bodyA(bodyA),
|
||||
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();
|
||||
}
|
||||
else
|
||||
m_jacSizeBoth = m_jacSizeA;
|
||||
|
||||
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
|
||||
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
|
||||
}
|
||||
|
||||
btMultiBodyConstraint::~btMultiBodyConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA,btScalar* jacOrgB,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar desiredVelocity,
|
||||
btScalar lowerLimit,
|
||||
btScalar upperLimit)
|
||||
{
|
||||
|
||||
|
||||
|
||||
constraintRow.m_multiBodyA = m_bodyA;
|
||||
constraintRow.m_multiBodyB = m_bodyB;
|
||||
|
||||
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
|
||||
const int ndofA = multiBodyA->getNumLinks() + 6;
|
||||
|
||||
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
if (constraintRow.m_deltaVelAindex <0)
|
||||
{
|
||||
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
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);
|
||||
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 (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
|
||||
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (constraintRow.m_deltaVelBindex <0)
|
||||
{
|
||||
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
constraintRow.m_jacBindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
|
||||
|
||||
for (int i=0;i<ndofB;i++)
|
||||
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
|
||||
|
||||
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);
|
||||
}
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* lambdaA =0;
|
||||
btScalar* lambdaB =0;
|
||||
int ndofA = 0;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
|
||||
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
btScalar j = jacA[i] ;
|
||||
btScalar l =lambdaA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
|
||||
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
btScalar j = jacB[i] ;
|
||||
btScalar l =lambdaB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
denom1 += jacB[i] * lambdaA[i];
|
||||
denom1 += jacA[i] * lambdaB[i];
|
||||
}
|
||||
}
|
||||
|
||||
btScalar d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
|
||||
constraintRow.m_jacDiagABInv = 1.f/(d);
|
||||
} else
|
||||
{
|
||||
constraintRow.m_jacDiagABInv = 1.f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining constraintRow fields
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = 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;
|
||||
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
}
|
||||
|
||||
constraintRow.m_friction = 0.f;
|
||||
|
||||
constraintRow.m_appliedImpulse = 0.f;
|
||||
constraintRow.m_appliedPushImpulse = 0.f;
|
||||
|
||||
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
|
||||
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse)
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
constraintRow.m_rhs = velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
constraintRow.m_rhs = velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = 0.f;
|
||||
}
|
||||
|
||||
|
||||
constraintRow.m_cfm = 0.f;
|
||||
constraintRow.m_lowerLimit = lowerLimit;
|
||||
constraintRow.m_upperLimit = upperLimit;
|
||||
|
||||
}
|
||||
return rel_vel;
|
||||
}
|
||||
|
||||
|
||||
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
|
||||
{
|
||||
for (int i = 0; i < ndof; ++i)
|
||||
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
|
||||
}
|
||||
|
||||
|
||||
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar position,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA, btScalar* jacOrgB,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar posError,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar lowerLimit, btScalar upperLimit,
|
||||
btScalar relaxation,
|
||||
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
|
||||
|
||||
btVector3 rel_pos1 = posAworld;
|
||||
btVector3 rel_pos2 = posBworld;
|
||||
|
||||
solverConstraint.m_multiBodyA = m_bodyA;
|
||||
solverConstraint.m_multiBodyB = m_bodyB;
|
||||
solverConstraint.m_linkA = m_linkA;
|
||||
solverConstraint.m_linkB = m_linkB;
|
||||
|
||||
solverConstraint.m_linkB = m_linkB;
|
||||
|
||||
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
|
||||
|
||||
const btVector3& pos1 = posAworld;
|
||||
const btVector3& pos2 = posBworld;
|
||||
|
||||
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
|
||||
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
|
||||
|
||||
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
|
||||
|
||||
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
|
||||
if (bodyA)
|
||||
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
|
||||
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
|
||||
if (bodyB)
|
||||
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
relaxation = 1.f;
|
||||
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
const int ndofA = multiBodyA->getNumLinks() + 6;
|
||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
@ -271,16 +93,37 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
|
||||
//resize..
|
||||
solverConstraint.m_jacAindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
//copy/determine
|
||||
if(jacOrgA)
|
||||
{
|
||||
for (int i=0;i<ndofA;i++)
|
||||
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
//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)
|
||||
//resize..
|
||||
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());
|
||||
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
} else
|
||||
//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);
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
@ -290,7 +133,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)
|
||||
@ -300,22 +143,43 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
|
||||
//resize..
|
||||
solverConstraint.m_jacBindex = data.m_jacobians.size();
|
||||
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
|
||||
//copy/determine..
|
||||
if(jacOrgB)
|
||||
{
|
||||
for (int i=0;i<ndofB;i++)
|
||||
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
|
||||
}
|
||||
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)
|
||||
//resize..
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
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);
|
||||
|
||||
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);
|
||||
} else
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -contactNormalOnB;
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
@ -323,121 +187,103 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* lambdaA =0;
|
||||
btScalar* lambdaB =0;
|
||||
btScalar* deltaVelA = 0;
|
||||
btScalar* deltaVelB = 0;
|
||||
int ndofA = 0;
|
||||
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
|
||||
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];
|
||||
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
btScalar j = jacA[i] ;
|
||||
btScalar l =lambdaA[i];
|
||||
btScalar l = deltaVelA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
{
|
||||
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
||||
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
||||
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
//
|
||||
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];
|
||||
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
btScalar j = jacB[i] ;
|
||||
btScalar l =lambdaB[i];
|
||||
btScalar l = deltaVelB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
} else
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
if (rb1)
|
||||
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
|
||||
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
|
||||
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
|
||||
denom1 += jacB[i] * deltaVelA[i];
|
||||
denom1 += jacA[i] * deltaVelB[i];
|
||||
}
|
||||
}
|
||||
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
denom1 += jacB[i] * lambdaA[i];
|
||||
denom1 += jacA[i] * lambdaB[i];
|
||||
}
|
||||
}
|
||||
|
||||
btScalar d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
//
|
||||
btScalar d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
|
||||
solverConstraint.m_jacDiagABInv = relaxation/(d);
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_jacDiagABInv = relaxation/(d);
|
||||
}
|
||||
else
|
||||
{
|
||||
solverConstraint.m_jacDiagABInv = 1.f;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining solverConstraint fields
|
||||
|
||||
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
|
||||
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
|
||||
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];
|
||||
} else
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
if (rb0)
|
||||
{
|
||||
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
|
||||
}
|
||||
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
|
||||
}
|
||||
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];
|
||||
|
||||
} else
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
if (rb1)
|
||||
{
|
||||
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
|
||||
}
|
||||
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
|
||||
}
|
||||
|
||||
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
|
||||
|
||||
|
||||
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (restitution <= btScalar(0.))
|
||||
{
|
||||
restitution = 0.f;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -474,17 +320,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
}
|
||||
} else
|
||||
*/
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
}
|
||||
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
{
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = restitution - rel_vel;// * damping;
|
||||
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
@ -493,15 +336,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
@ -520,8 +355,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
}
|
||||
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
|
||||
solverConstraint.m_upperLimit = m_maxAppliedImpulse;
|
||||
solverConstraint.m_lowerLimit = lowerLimit;
|
||||
solverConstraint.m_upperLimit = upperLimit;
|
||||
}
|
||||
|
||||
return rel_vel;
|
||||
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -66,22 +66,16 @@ protected:
|
||||
|
||||
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
|
||||
|
||||
void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar position,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA,btScalar* jacOrgB,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar desiredVelocity,
|
||||
btScalar lowerLimit,
|
||||
btScalar upperLimit);
|
||||
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA, btScalar* jacOrgB,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar posError,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar lowerLimit, btScalar upperLimit,
|
||||
btScalar relaxation = 1.f,
|
||||
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
public:
|
||||
|
||||
@ -99,7 +93,7 @@ public:
|
||||
|
||||
int getNumRows() const
|
||||
{
|
||||
return m_num_rows;
|
||||
return m_numRows;
|
||||
}
|
||||
|
||||
btMultiBody* getMultiBodyA()
|
||||
@ -116,12 +110,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 +129,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
|
||||
|
||||
@ -36,6 +36,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
//if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
//resolveSingleConstraintRowGenericMultiBody(constraint);
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
|
||||
if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
|
||||
}
|
||||
|
||||
//solve featherstone normal contact
|
||||
@ -44,6 +46,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
|
||||
if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
|
||||
if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
|
||||
}
|
||||
|
||||
//solve featherstone frictional contact
|
||||
@ -60,6 +65,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
|
||||
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
|
||||
resolveSingleConstraintRowGeneric(frictionConstraint);
|
||||
|
||||
if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->__posUpdated = false;
|
||||
if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->__posUpdated = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -108,10 +116,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 +127,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 +159,13 @@ 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
|
||||
//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->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 +173,13 @@ 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
|
||||
//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->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 +198,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 +275,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 +295,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 +314,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 +330,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 +358,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 +377,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 +434,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 +447,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];
|
||||
@ -432,12 +462,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
|
||||
|
||||
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (restitution <= btScalar(0.))
|
||||
if(!isFriction)
|
||||
{
|
||||
restitution = 0.f;
|
||||
};
|
||||
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (restitution <= btScalar(0.))
|
||||
{
|
||||
restitution = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -452,7 +484,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 +498,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
|
||||
{
|
||||
@ -479,11 +517,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
@ -494,7 +530,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
velocityError -= penetration / infoGlobal.m_timeStep;
|
||||
|
||||
} else
|
||||
{
|
||||
@ -504,22 +540,33 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
if(!isFriction)
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
}
|
||||
else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = penetrationImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
|
||||
solverConstraint.m_upperLimit = solverConstraint.m_friction;
|
||||
}
|
||||
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
|
||||
}
|
||||
|
||||
}
|
||||
@ -702,6 +749,10 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
||||
{
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
@ -709,10 +760,6 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
|
||||
{
|
||||
cp.m_lateralFrictionInitialized = true;
|
||||
|
||||
@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
|
||||
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
|
||||
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
|
||||
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
|
||||
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
|
||||
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
|
||||
|
||||
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
|
||||
|
||||
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
|
||||
m_bodies.resize(0);
|
||||
@ -392,9 +394,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
|
||||
delete m_solverMultiBodyIslandCallback;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
@ -451,7 +450,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 +463,170 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
||||
}
|
||||
|
||||
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
|
||||
bool doNotUpdatePos = false;
|
||||
|
||||
if(bod->isMultiDof())
|
||||
{
|
||||
if(!bod->isUsingRK4Integration())
|
||||
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
|
||||
else
|
||||
{
|
||||
//
|
||||
int numDofs = bod->getNumDofs() + 6;
|
||||
int numPosVars = bod->getNumPosVars() + 7;
|
||||
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
|
||||
//convenience
|
||||
btScalar *pMem = &scratch_r2[0];
|
||||
btScalar *scratch_q0 = pMem; pMem += numPosVars;
|
||||
btScalar *scratch_qx = pMem; pMem += numPosVars;
|
||||
btScalar *scratch_qd0 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qd1 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qd2 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qd3 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qdd0 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qdd1 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qdd2 = pMem; pMem += numDofs;
|
||||
btScalar *scratch_qdd3 = pMem; pMem += numDofs;
|
||||
btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
|
||||
|
||||
/////
|
||||
//copy q0 to scratch_q0 and qd0 to scratch_qd0
|
||||
scratch_q0[0] = bod->getWorldToBaseRot().x();
|
||||
scratch_q0[1] = bod->getWorldToBaseRot().y();
|
||||
scratch_q0[2] = bod->getWorldToBaseRot().z();
|
||||
scratch_q0[3] = bod->getWorldToBaseRot().w();
|
||||
scratch_q0[4] = bod->getBasePos().x();
|
||||
scratch_q0[5] = bod->getBasePos().y();
|
||||
scratch_q0[6] = bod->getBasePos().z();
|
||||
//
|
||||
for(int link = 0; link < bod->getNumLinks(); ++link)
|
||||
{
|
||||
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
|
||||
scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
|
||||
}
|
||||
//
|
||||
for(int dof = 0; dof < numDofs; ++dof)
|
||||
scratch_qd0[dof] = bod->getVelocityVector()[dof];
|
||||
////
|
||||
struct
|
||||
{
|
||||
btMultiBody *bod;
|
||||
btScalar *scratch_qx, *scratch_q0;
|
||||
|
||||
void operator()()
|
||||
{
|
||||
for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
|
||||
scratch_qx[dof] = scratch_q0[dof];
|
||||
}
|
||||
} pResetQx = {bod, scratch_qx, scratch_q0};
|
||||
//
|
||||
struct
|
||||
{
|
||||
void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
|
||||
{
|
||||
for(int i = 0; i < size; ++i)
|
||||
pVal[i] = pCurVal[i] + dt * pDer[i];
|
||||
}
|
||||
|
||||
} pEulerIntegrate;
|
||||
//
|
||||
struct
|
||||
{
|
||||
void operator()(btMultiBody *pBody, const btScalar *pData)
|
||||
{
|
||||
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
|
||||
|
||||
for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
|
||||
pVel[i] = pData[i];
|
||||
|
||||
}
|
||||
} pCopyToVelocityVector;
|
||||
//
|
||||
struct
|
||||
{
|
||||
void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
|
||||
{
|
||||
for(int i = 0; i < size; ++i)
|
||||
pDst[i] = pSrc[start + i];
|
||||
}
|
||||
} pCopy;
|
||||
//
|
||||
|
||||
btScalar h = solverInfo.m_timeStep;
|
||||
#define output &scratch_r[bod->getNumDofs()]
|
||||
//calc qdd0 from: q0 & qd0
|
||||
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
|
||||
pCopy(output, scratch_qdd0, 0, numDofs);
|
||||
//calc q1 = q0 + h/2 * qd0
|
||||
pResetQx();
|
||||
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
|
||||
//calc qd1 = qd0 + h/2 * qdd0
|
||||
pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
|
||||
//
|
||||
//calc qdd1 from: q1 & qd1
|
||||
pCopyToVelocityVector(bod, scratch_qd1);
|
||||
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
|
||||
pCopy(output, scratch_qdd1, 0, numDofs);
|
||||
//calc q2 = q0 + h/2 * qd1
|
||||
pResetQx();
|
||||
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
|
||||
//calc qd2 = qd0 + h/2 * qdd1
|
||||
pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
|
||||
//
|
||||
//calc qdd2 from: q2 & qd2
|
||||
pCopyToVelocityVector(bod, scratch_qd2);
|
||||
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
|
||||
pCopy(output, scratch_qdd2, 0, numDofs);
|
||||
//calc q3 = q0 + h * qd2
|
||||
pResetQx();
|
||||
bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
|
||||
//calc qd3 = qd0 + h * qdd2
|
||||
pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
|
||||
//
|
||||
//calc qdd3 from: q3 & qd3
|
||||
pCopyToVelocityVector(bod, scratch_qd3);
|
||||
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
|
||||
pCopy(output, scratch_qdd3, 0, numDofs);
|
||||
|
||||
//
|
||||
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
|
||||
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
|
||||
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
|
||||
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
|
||||
for(int i = 0; i < numDofs; ++i)
|
||||
{
|
||||
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
|
||||
delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
|
||||
//delta_q[i] = h*scratch_qd0[i];
|
||||
//delta_qd[i] = h*scratch_qdd0[i];
|
||||
}
|
||||
//
|
||||
pCopyToVelocityVector(bod, scratch_qd0);
|
||||
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
|
||||
//
|
||||
if(!doNotUpdatePos)
|
||||
{
|
||||
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
|
||||
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
|
||||
|
||||
for(int i = 0; i < numDofs; ++i)
|
||||
pRealBuf[i] = delta_q[i];
|
||||
|
||||
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
|
||||
bod->__posUpdated = true;
|
||||
}
|
||||
|
||||
//ugly hack which resets the cached data to t0 (needed for constraint solver)
|
||||
{
|
||||
for(int link = 0; link < bod->getNumLinks(); ++link)
|
||||
bod->getLink(link).updateCacheMultiDof();
|
||||
bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
else
|
||||
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -503,13 +666,25 @@ 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())
|
||||
{
|
||||
if(!bod->__posUpdated)
|
||||
bod->stepPositionsMultiDof(timeStep);
|
||||
else
|
||||
{
|
||||
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
|
||||
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
|
||||
|
||||
|
||||
bod->stepPositionsMultiDof(1, 0, pRealBuf);
|
||||
bod->__posUpdated = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
bod->stepPositions(timeStep);
|
||||
|
||||
world_to_local[0] = bod->getWorldToBaseRot();
|
||||
local_origin[0] = bod->getBasePos();
|
||||
|
||||
@ -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));
|
||||
@ -89,8 +99,10 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
constraintRow.m_multiBodyA = m_bodyA;
|
||||
constraintRow.m_multiBodyB = m_bodyB;
|
||||
const btScalar posError = 0; //why assume it's zero?
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
|
||||
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
|
||||
{
|
||||
btScalar penetration = getPosition(row);
|
||||
btScalar positionalError = 0.f;
|
||||
|
||||
@ -21,10 +21,13 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
||||
:btMultiBodyConstraint(body,body,link,link,1,true),
|
||||
m_desiredVelocity(desiredVelocity)
|
||||
{
|
||||
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
||||
|
||||
m_maxAppliedImpulse = maxMotorImpulse;
|
||||
// the data.m_jacobians never change, so may as well
|
||||
// initialize them here
|
||||
@ -32,8 +35,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 + linkDoF : link);
|
||||
|
||||
// row 0: the lower bound
|
||||
// row 0: the lower bound
|
||||
jacobianA(0)[offset] = 1;
|
||||
}
|
||||
btMultiBodyJointMotor::~btMultiBodyJointMotor()
|
||||
{
|
||||
@ -76,13 +82,15 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
|
||||
// directions were set in the ctor and never change.
|
||||
|
||||
|
||||
const btScalar posError = 0;
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
for (int row=0;row<getNumRows();row++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
|
||||
btScalar penetration = 0;
|
||||
fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
|
||||
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -30,7 +30,7 @@ protected:
|
||||
|
||||
public:
|
||||
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
virtual ~btMultiBodyJointMotor();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
|
||||
@ -24,6 +24,327 @@ 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 btScalar &s) { m_topVec *= s; m_bottomVec *= s; 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));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformInverseRotationOnly( 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;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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 +354,159 @@ 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
|
||||
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
||||
|
||||
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.
|
||||
enum eFeatherstoneJointType
|
||||
{
|
||||
eRevolute = 0,
|
||||
ePrismatic = 1,
|
||||
eSpherical = 2,
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
ePlanar = 3,
|
||||
#endif
|
||||
eInvalid
|
||||
};
|
||||
|
||||
btVector3 applied_force; // In WORLD frame
|
||||
btVector3 applied_torque; // In WORLD frame
|
||||
btScalar joint_torque;
|
||||
eFeatherstoneJointType m_jointType;
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
|
||||
// "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, m_cfgOffset;
|
||||
|
||||
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(btScalar *pq = 0)
|
||||
{
|
||||
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
|
||||
|
||||
switch(m_jointType)
|
||||
{
|
||||
case eRevolute:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[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 + pJointPos[0] * getAxisBottom(0);
|
||||
|
||||
break;
|
||||
}
|
||||
case eSpherical:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case ePlanar:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -19,8 +19,14 @@ subject to the following restrictions:
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
#define BTMBP2PCONSTRAINT_DIM 3
|
||||
#else
|
||||
#define BTMBP2PCONSTRAINT_DIM 6
|
||||
#endif
|
||||
|
||||
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,BTMBP2PCONSTRAINT_DIM,false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
@ -29,7 +35,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
|
||||
}
|
||||
|
||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
|
||||
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
@ -90,7 +96,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
|
||||
{
|
||||
|
||||
// int i=1;
|
||||
for (int i=0;i<3;i++)
|
||||
for (int i=0;i<BTMBP2PCONSTRAINT_DIM;i++)
|
||||
{
|
||||
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
@ -98,9 +104,12 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
|
||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||
|
||||
|
||||
btVector3 contactNormalOnB(0,0,0);
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
contactNormalOnB[i] = -1;
|
||||
#else
|
||||
contactNormalOnB[i%3] = -1;
|
||||
#endif
|
||||
|
||||
btScalar penetration = 0;
|
||||
|
||||
@ -127,17 +136,35 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
|
||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
|
||||
}
|
||||
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
|
||||
btScalar relaxation = 1.f;
|
||||
fillMultiBodyConstraintMixed(constraintRow, data,
|
||||
contactNormalOnB,
|
||||
pivotAworld, pivotBworld,
|
||||
position,
|
||||
infoGlobal,
|
||||
relaxation,
|
||||
false);
|
||||
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
|
||||
constraintRow.m_upperLimit = m_maxAppliedImpulse;
|
||||
|
||||
btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
|
||||
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0,
|
||||
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse
|
||||
);
|
||||
#else
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
btAssert(m_bodyA->isMultiDof());
|
||||
|
||||
btScalar* jac1 = jacobianA(i);
|
||||
const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
|
||||
const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
|
||||
|
||||
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
|
||||
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
|
||||
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse
|
||||
);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -20,6 +20,8 @@ subject to the following restrictions:
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
|
||||
class btMultiBodyPoint2Point : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
@ -28,16 +28,19 @@ 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;
|
||||
btVector3 m_contactNormal1;
|
||||
int m_jacAindex;
|
||||
|
||||
int m_deltaVelBindex;
|
||||
int m_jacBindex;
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
int m_jacBindex;
|
||||
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user