public void UpdatePosition()
{
if (!body1IsMassPoint)
{
JVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
}
JVector.Add(ref p1, ref body1.position, out p1);
if (!body2IsMassPoint)
{
JVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
}
JVector.Add(ref p2, ref body2.position, out p2);
JVector dist; JVector.Subtract(ref p1, ref p2, out dist);
penetration = JVector.Dot(ref dist, ref normal);
}