I've been trying to make a ball and socket joint based on constraint impulses. I do this by calculating the relative velocities at the joint ends on the two connected objects and using this to find an impulse that will make this relative velocity zero. I also add a drift velocity to the relative velocity so that the impulse also corrects drift. The problem is the joint is separating like in the screen shot below.
I was hoping someone could have a look at my code and offer some suggestions about why this would be happening as I have no idea where to look.
Code: Select all
void cBallSocketJoint::PreApply(float dt)
{
// Calculate the deviation between the end points and the joint anchor
cVector deviation = m_a->WorldPos(m_ra) - m_b->WorldPos(m_rb);
float deviationMag = deviation.Mag();
if (deviationMag > MAX_DEVIATION)
{
cVector deviationDir = deviation.Unit();
m_vrExtra = deviationDir * (deviationMag / dt);
}
else
{
m_vrExtra.Zero();
}
}
void cBallSocketJoint::Apply(float dt)
{
// Align joint ends based on current rotation
cVector ra = m_a->Q() * m_ra;
cVector rb = m_b->Q() * m_rb;
// Get velocity of joint ends
cVector va = m_a->LocalPointVel(ra);
cVector vb = m_b->LocalPointVel(rb);
cVector vr = m_vrExtra + va - vb;
if (vr.Mag() < 0.0001f)
return;
cVector normal = vr.Unit();
float num = -Dot(normal, vr);
float term1 = 1.0f / m_a->Mass();
float term2 = 1.0f / m_b->Mass();
float term3 = Dot(normal, Cross(m_a->IInv() * Cross(ra, normal), ra));
float term4 = Dot(normal, Cross(m_b->IInv() * Cross(rb, normal), rb));
float den = term1 + term2 + term3 + term4;
if (absf(den) < 0.0001f)
return;
cVector impulse = normal * (num / den);
// Apply constraint impulse
m_a->ApplyLocalImpulse(impulse, ra);
m_b->ApplyLocalImpulse(-impulse, rb);
}
Mark.