/// Initialize the bodies, anchors, axis, and reference angle using the world /// anchor and world axis. // Linear constraint (point-to-line) // d = pB - pA = xB + rB - xA - rA // C = dot(ay, d) // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] // Spring linear constraint // C = dot(ax, d) // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] // Motor rotational constraint // Cdot = wB - wA // J = [0 0 -1 0 0 1] public void Initialize(Body bA, Body bB, Vec2 anchor, Vec2 axis) { bodyA = bA; bodyB = bB; localAnchorA = bodyA.GetLocalPoint(anchor); localAnchorB = bodyB.GetLocalPoint(anchor); localAxisA = bodyA.GetLocalVector(axis); }