/// Initialize the bodies, anchors, and reference angle using a world /// anchor point. // 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 bA, Body bB, Vec2 anchor) { bodyA = bA; bodyB = bB; localAnchorA = bodyA.GetLocalPoint(anchor); localAnchorB = bodyB.GetLocalPoint(anchor); referenceAngle = bodyB.GetAngle() - bodyA.GetAngle(); }
/// Initialize the bodies, anchors, axis, and reference angle using the world /// anchor and world axis. // Point-to-point constraint // 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 // Cdot = w2 - w1 // J = [0 0 -1 0 0 1] // K = invI1 + invI2 public void Initialize(Body bA, Body bB, Vec2 anchor) { bodyA = bA; bodyB = bB; localAnchorA = bodyA.GetLocalPoint(anchor); localAnchorB = bodyB.GetLocalPoint(anchor); }
/// 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); }
// 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, axis, and reference angle using the world /// anchor and world axis. public void Initialize(Body b1, Body b2, Vector2 anchor, Vector2 axis) { bodyA = b1; bodyB = b2; localAnchorA = bodyA.GetLocalPoint(anchor); localAnchorB = bodyB.GetLocalPoint(anchor); localAxisA = bodyA.GetLocalVector(axis); }
// Point-to-point constraint // 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 // Cdot = w2 - w1 // J = [0 0 -1 0 0 1] // K = invI1 + invI2 public void Initialize(Body b1, Body b2, Vector2 anchor1, Vector2 anchor2) { bodyA = b1; bodyB = b2; localAnchorA = bodyA.GetLocalPoint(anchor1); localAnchorB = bodyB.GetLocalPoint(anchor2); }
/// Initialize the bodies, anchors, and length using the world /// anchors. // 1-D constrained system // m (v2 - v1) = lambda // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. // x2 = x1 + h * v2 // 1-D mass-damper-spring system // m (v2 - v1) + h * d * v2 + h * k * // C = norm(p2 - p1) - L // u = (p2 - p1) / norm(p2 - p1) // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) // J = [-u -cross(r1, u) u cross(r2, u)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 public void Initialize(Body b1, Body b2, Vec2 anchor1, Vec2 anchor2) { bodyA = b1; bodyB = b2; localAnchorA = bodyA.GetLocalPoint(anchor1); localAnchorB = bodyB.GetLocalPoint(anchor2); Vec2 d = anchor2 - anchor1; length = d.Length(); }
/// Initialize the bodies, anchors, and length using the world /// anchors. // 1-D rained system // m (v2 - v1) = lambda // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. // x2 = x1 + h * v2 // 1-D mass-damper-spring system // m (v2 - v1) + h * d * v2 + h * k * // C = norm(p2 - p1) - L // u = (p2 - p1) / norm(p2 - p1) // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) // J = [-u -cross(r1, u) u cross(r2, u)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 public void Initialize(Body b1, Body b2, Vector2 anchor1, Vector2 anchor2, float maxlength) { bodyA = b1; bodyB = b2; localAnchorA = bodyA.GetLocalPoint(anchor1); localAnchorB = bodyB.GetLocalPoint(anchor2); length = maxlength; }
/// Initialize the bodies and offsets using the current transforms. // Point-to-point constraint // 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 // Cdot = w2 - w1 // J = [0 0 -1 0 0 1] // K = invI1 + invI2 public void Initialize(Body bA, Body bB) { bodyA = bA; bodyB = bB; Vec2 xB = bodyB.GetPosition(); linearOffset = bodyA.GetLocalPoint(xB); float angleA = bodyA.GetAngle(); float angleB = bodyB.GetAngle(); angularOffset = angleB - angleA; }
/// Initialize the bodies, anchors, and length using the world /// anchors. // 1-D rained system // m (v2 - v1) = lambda // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. // x2 = x1 + h * v2 // 1-D mass-damper-spring system // m (v2 - v1) + h * d * v2 + h * k * // C = norm(p2 - p1) - L // u = (p2 - p1) / norm(p2 - p1) // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) // J = [-u -cross(r1, u) u cross(r2, u)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 public void Initialize(Body b1, Body b2, Vector2 anchor1, Vector2 anchor2) { bodyA = b1; bodyB = b2; localAnchorA = bodyA.GetLocalPoint(anchor1); localAnchorB = bodyB.GetLocalPoint(anchor2); Vector2 d = anchor2 - anchor1; length = d.magnitude; }
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. // Pulley: // length1 = norm(p1 - s1) // length2 = norm(p2 - s2) // C0 = (length1 + ratio * length2)_initial // C = C0 - (length1 + ratio * length2) // u1 = (p1 - s1) / norm(p1 - s1) // u2 = (p2 - s2) / norm(p2 - s2) // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) public void Initialize(Body bA, Body bB, Vec2 groundA, Vec2 groundB, Vec2 anchorA, Vec2 anchorB, float r) { bodyA = bA; bodyB = bB; groundAnchorA = groundA; groundAnchorB = groundB; localAnchorA = bodyA.GetLocalPoint(anchorA); localAnchorB = bodyB.GetLocalPoint(anchorB); Vec2 dA = anchorA - groundA; lengthA = dA.Length(); Vec2 dB = anchorB - groundB; lengthB = dB.Length(); ratio = r; Utilities.Assert(ratio > Single.Epsilon); }
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. public void Initialize(Body b1, Body b2, Vector2 ga1, Vector2 ga2, Vector2 anchor1, Vector2 anchor2, float r) { bodyA = b1; bodyB = b2; groundAnchorA = ga1; groundAnchorB = ga2; localAnchorA = bodyA.GetLocalPoint(anchor1); localAnchorB = bodyB.GetLocalPoint(anchor2); Vector2 d1 = anchor1 - ga1; lengthA = d1.magnitude; Vector2 d2 = anchor2 - ga2; lengthB = d2.magnitude; ratio = r; //Debug.Assert(ratio > Settings.b2_epsilon); float C = lengthA + ratio * lengthB; maxLengthA = C - ratio * b2_minPulleyLength; maxLengthB = (C - b2_minPulleyLength) / ratio; }