public JacobianEntry(
ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 rel_pos1,ref IndexedVector3 rel_pos2,
ref IndexedVector3 jointAxis,
ref IndexedVector3 inertiaInvA,
float massInvA,
ref IndexedVector3 inertiaInvB,
float massInvB)
{
m_linearJointAxis = jointAxis;
m_aJ = world2A * (rel_pos1.Cross(ref m_linearJointAxis));
m_bJ = world2B * (rel_pos2.Cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + IndexedVector3.Dot(m_0MinvJt,m_aJ) + massInvB + IndexedVector3.Dot(m_1MinvJt,m_bJ);
Debug.Assert(m_Adiag > 0.0f);
}
