Physics Simulation Forum

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);
}