// 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(); }