/// 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, 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.
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 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, 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.
// 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;
}
