/// <summary>
/// Initialize the bodies, anchors, and length using the world anchors.
/// </summary>
/// <param name="b1">First body</param>
/// <param name="b2">Second body</param>
/// <param name="anchor1">World anchor on first body</param>
/// <param name="anchor2">World anchor on second body</param>
public void Initialize(Body b1, Body b2, Vec2 anchor1, Vec2 anchor2)
{
BodyA = b1;
BodyB = b2;
LocalAnchorA.Set(BodyA.GetLocalPoint(anchor1));
LocalAnchorB.Set(BodyB.GetLocalPoint(anchor2));
Vec2 d = anchor2.Sub(anchor1);
Length = d.Length();
}