I have coded the following distance constraint but am having problems with the bias, since the relative position can sometimes be zero, here is the code, please help !
I have tested it without the bias and it still behaves like a point-to-point constraint
Thanks
Code: Select all
AMG3DDistanceJoint::AMG3DDistanceJoint(AMG3DRigidBody *pRigidBody1, AMG3DRigidBody *pRigidBody2, AMG3DVector4 vAnchorPoint)
{
m_pRigidBody1 = pRigidBody1;
m_pRigidBody2 = pRigidBody2;
AMG3DMatrix4x4 Rot1, Rot1T, Rot2, Rot2T;
AMG3DQuaternion q1 = m_pRigidBody1->quatOrientation;
AMG3DQuaternion q2 = m_pRigidBody2->quatOrientation;
q1.toMatrix(&Rot1);
Rot1T.transposeOf(Rot1);
q2.toMatrix(&Rot2);
Rot2T.transposeOf(Rot2);
localAnchor1 = Rot1T*(vAnchorPoint - m_pRigidBody1->position);
localAnchor2 = Rot2T*(vAnchorPoint - m_pRigidBody2->position);
initialDistance = (localAnchor1-localAnchor2).magnitude();
}
void AMG3DDistanceJoint::preStep(AMG3DScalar dt)
{
if(dt<=0.0f)
return;
AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_PositionCorrection) ? 0.2f : 0.0f;
AMG3DMatrix4x4 Rot1, Rot2;
AMG3DQuaternion q1 = m_pRigidBody1->quatOrientation;
AMG3DQuaternion q2 = m_pRigidBody2->quatOrientation;
q1.toMatrix(&Rot1);
q2.toMatrix(&Rot2);
AMG3DVector4 ra = Rot1*localAnchor1;
AMG3DVector4 rb = Rot2*localAnchor2;
AMG3DMatrix3x3 I;
I.identity();
// Compute the bias factor to prevent drift
AMG3DVector4 dp = m_pRigidBody1->position + ra - m_pRigidBody2->position - rb; // Relative position
AMG3DScalar C = dp.magnitude() - initialDistance;
bias = k_biasFactor / dt * C;
dp.normalize();
n = dp; // n = dp/|dp|
InvMa = I*m_pRigidBody1->invMass; // Ma^-1
SkewRa = ra.skew(); // [~ra]
AMG3DMatrix3x3 InvIa = m_pRigidBody1->invIWorld; // Ia^-1
SkewRaT.transposeOf(SkewRa); // [~ra]T
InvMb = I*m_pRigidBody2->invMass; // Mb^-1
SkewRb = rb.skew(); // [~rb]
AMG3DMatrix3x3 InvIb = m_pRigidBody2->invIWorld; // Ib^-1
SkewRbT.transposeOf(SkewRb); // [~rb]T
// a = JM^-1JT
// a = n.(Ma^-1*n + [~ra]T*Ia^-1*n*[~ra] + Mb^-1*n + [~rb]T*Ib^-1*n*[~rb])
a = n.dot(InvMa*n + SkewRaT*InvIa*n*SkewRa + InvMb*n + SkewRbT*InvIb*n*SkewRb);
}
void AMG3DDistanceJoint::applyImpulse()
{
AMG3DVector4 va = m_pRigidBody1->velocity;
AMG3DVector4 wa = m_pRigidBody1->angularVelocity;
AMG3DVector4 vb = m_pRigidBody2->velocity;
AMG3DVector4 wb = m_pRigidBody2->angularVelocity;
// b = -Jvi - beta/h*C
AMG3DScalar b = n.reverseOf().dot(va + SkewRaT*wa - vb - SkewRbT*wb) - bias;
// x = lambda (corrective impulse)
lambda = a>0 ? b/a : 0;
P += lambda; // Accumulate impulse for warm starting (To Do: Add warm starting)
// vfa = via + Ma^-1*n*1*lambda
// wfa = wia + Ia^-1-n*[~ra]*lambda
// vfb = vib + Mb^-1*n*-1*lambda
// wfb = wib + Ib^-1*n*-[~rb]*lambda
m_pRigidBody1->velocity += InvMa*n*lambda;
m_pRigidBody1->angularVelocity += m_pRigidBody1->invIWorld*n*SkewRa*lambda;
m_pRigidBody2->velocity -= InvMb*n*lambda;
m_pRigidBody2->angularVelocity -= m_pRigidBody2->invIWorld*n*SkewRb*lambda;
}