Code:

void cBallSocketJoint::PreApply(float dt){// Calculate the deviation between the end points and the joint anchorcVector 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 rotationcVector ra = m_a->Q() * m_ra;cVector rb = m_b->Q() * m_rb;// Get velocity of joint endscVector 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 impulsem_a->ApplyLocalImpulse(impulse, ra);m_b->ApplyLocalImpulse(-impulse, rb);}