// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// C = angle2 - angle1 - referenceAngle
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
public void Initialize(Body b1, Body b2, Vector2 anchor)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
}
/// Initialize the bodies, anchors, and reference angle using the world
/// anchor.
public void Initialize(Body b1, Body b2, Vector2 anchor)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
}