The line constraint is a constraint that restricts the movement of a pair of bodies to a prescribed "line". Dyn4j sites a car on a roller coaster track as being an example.
It is similar to the Prismatic constraint but the bodies are free to rotate about the anchor point.
The initial vector u will be supplied in the construction of the constraint.
From u, we will obtain the tangent of u, the t vector.
Each simulation step we will recompute u from the anchor points and use it along with the saved t vector to determine if the constraint has been violated.
class LineJoint extends Joint {
constructor(b1, b2, anchor1 = b1.position, anchor2 = b2.position, dir, frequency = 30, dampingRatio = 1.0, jointMass = -1) {
super(b1, b2, frequency, dampingRatio, jointMass);
this.impulseSum = 0.0;
if (body1.type == Type.Static && body2.type == Type.Static)
throw "Can't make line constraint between static bodies";
if (body1.type == Type.Dynamic && body2.type == Type.Dynamic)
throw "Can't make line constraint between dynamic bodies";
if (body2.type == Type.Static)
throw "Please make line constraint by using the body1 as a static body";
this.localAnchor1 = Vector2.InverseRotateAndTranslate(this.body1.Rot, this.body1.position, anchor1);
this.localAnchor2 = Vector2.InverseRotateAndTranslate(this.body2.Rot, this.body2.position, anchor2);
let u = anchor2.sub(anchor1);
this.t = new Vector2(-u.y, u.x).normalized();
if (dir == undefined) {
let u = anchor2.sub(anchor1);
this.t = new Vector2(-u.y, u.x).normalized();
}
else {
this.t = new Vector2(-dir.y, dir.x).normalized();
}
Util.assert(this.t.squaredLength > 0);
}
Position Constraint Function
The position constraint functino is defined as follows:
where
This equation states that any motion that is not along the vector is invalid, because the tangent of that motion projected onto (via the dot product) the t vector will no longer yield 0.
Velocity Constraint Function
By the chain rule:
Where the derivative of u:
And the derivative of t:
which gives us the following constraint function
if we multiply this out, we get
If we rely on the triple product identity, we can rearrange the above as
Isolate The Velocities
by inspection the jacobian is
K-matrix
PreStep()
preStep(inv_dt) {
// Calculate Jacobian J and effective mass M
// J = [-t^t, -(ra + u)×t, t^t, rb×t]
// M = (J · M^-1 · J^t)^-1
// Pre-compute anchors, mass matrix, and bias.
Vector2.Rotate(this.body1.Rot, this.localAnchor1, this.r1);
Vector2.Rotate(this.body2.Rot, this.localAnchor2, this.r2);
let p1 = this.body1.position.add(this.r1);
let p2 = this.body2.position.add(this.r2);
this.u = p2.sub(p1);
let k = this.body2.invMass + this.r2.cross(this.t) * this.body2.invI
- (this.body1.invMass + this.r1.add(this.u).cross(this.t) * this.body1.invI)
+ this.gamma;
this.m = 1.0 / k;
let error = this.u.dot(this.t);
if (Settings.positionCorrection)
this.bias = error * this.beta * inv_dt;
else
this.bias = 0.0;
if (Settings.warmStarting)
this.applyImpulse(this.impulseSum);
}ApplyImpulse
solve() {
// Calculate corrective impulse: Pc
// Pc = J^t · λ (λ: lagrangian multiplier)
// λ = (J · M^-1 · J^t)^-1 ⋅ -(J·v+b)
let jv = this.t.dot(this.body2.linearVelocity) + this.r2.cross(this.t) * this.body2.angularVelocity
- (this.t.dot(this.body1.linearVelocity) + this.r2.add(this.u).cross(this.t) * this.body1.angularVelocity);
let lambda = this.m * -(jv + this.bias + this.impulseSum * this.gamma);
this.applyImpulse(lambda);
if (Settings.warmStarting)
this.impulseSum += lambda;
}
Comments