public void Initialize(b2Body b1, b2Body b2,
b2Vec2 anchor1, b2Vec2 anchor2)
{
BodyA = b1;
BodyB = b2;
localAnchorA = BodyA.GetLocalPoint(anchor1);
localAnchorB = BodyB.GetLocalPoint(anchor2);
b2Vec2 d = anchor2 - anchor1;
length = d.Length;
}