A joint is a form of constrained dynamics. Just as with non-penetration constraint, joint is a constraint whereby specific points of a pair of bodies are "fixed".
There are many different types of joint constraints. This post is about the Revolute Joint which comes with Box2D-Lite.
What is a Revolute Joint?
Applying the Generalized Constraint Framework
Implementation in Javascript
Derivation of the Jacobian Matrix
What is a Revolute Joint?
The revolute joint can be thought of as a hinge, a pin, or an axle.
Two bodies are allowed to rotate about a common point. In constrained dynamics speak, the number of degrees of freedom of one body has been reduced from 3 to 1, ie 2 degrees of freedom have been removed by the constraint.
An example would be the following whereby, the two bodies are "fixed" to the point indicated by the black circle. As Body 1 is given infinite mass, it is static. The red body will "swing" around the point.
where,
x1 = position of body1 (center of mass) in world space
r1 = radii from the center of mass of body1 (world space) to the contact point (world space)
x2 = position of body2 (center of mass) in world space
r2 = radii from the center of mass of body2 (world space) to the contact point (world space)
Box2D-Lite Joint Object
In Box2D-Lite, to create a joint, two bodies to be "joined" and a common anchor point around which the bodies can freely rotate is passed. The necessary information is encapsulted in an object called Joint.
Anchors in local space
Critical to Revolute joints is the concept of anchors.
In the Box2D Joint object, localAncor1, and localAnchor2 hold the anchor information for the bodies (i.e. r1 and r2 vectors in the diagram above) in local space (anchor - body.position, in world space, translated to local space).
void Joint::Set(Body* b1, Body* b2, const Vec2& anchor)
{
body1 = b1;
body2 = b2;
Mat22 Rot1(body1->rotation);
Mat22 Rot2(body2->rotation);
Mat22 Rot1T = Rot1.Transpose();
Mat22 Rot2T = Rot2.Transpose();
localAnchor1 = Rot1T * (anchor - body1->position);
localAnchor2 = Rot2T * (anchor - body2->position);
P.Set(0.0f, 0.0f);
softness = 0.0f;
biasFactor = 0.2f;
}Deriving the Jacobian Matrix
Position constraint function
Refer back to the diagram at the beginning of the post, and note that:
p = common anchor point defined in world space
x = center of mass = body.position in world space
r = vector pointing from body.position to the anchor point - this is defined in local space
It follows that p can be defined as
where R is the rotation matrix that transforms from local to world space (remember r is defined in local space).
In the real world, p would one single point. In the world of computer simulations, p, as defined this way (i.e. with respect to the center of mass of the body), not only can it move but 'it' can drive apart. To describe this, we can define the following:
What we are trying to do is to make sure that p1 and p2 remained fixed in place. This can be described as:
which can then be expressed as follows.
This is the position constraint function for a revolute joint. Note that this position constraint is satisfied when C is exactly equal to zero and broken if C is not zero. In contrast to the contact constraint which is an inequality constraint, Revolute Joint is an equality constraint.
Velocity constraint function
We can then arrive at the velocity constraint function by performing a derivative with respect to time on the above. The derivate with respect to time of position is simply the linear velocity. In order to perform a derivative of the angular component which involves a cross product, we can make use of the product rule which states:
Then, we can say that the derivative of the constraint position function is as follows:
Note that the above equation is stating that the relative velocity at the anchor point should be kept to zero.
Contrast with the contact constraint which was requiring the component of the relative velocity at the contact point along the normal to be kept non-negative.and also note, that this is same velocity constraint function, as explained in the Box2D-Lite docs. So far so good.
Deriving the Jacobian
Let us go onto derive the Jacobian matrix from the above, by employing some matrix multiplication and separating the velocities from the known coefficients.
As we have already seen in the contact constraint example, the cross product of two vectors can be transformed into a matrix multiplication as follows.
where [] is an operator that transforms a vector A=(a1,a2,a3) into a skew-symmetric matrix as follows.
Things are a bit convoluted when working in 2D, as angular velocity is a scalar. If we consider the angular velocity as a 3d vector (since the scalar angular velocity can be considered as representing the magnitude of the angular velocity around the z-axis pointing out of the screen), we can use the following "hack" to change the cross product of ω and the radius r into the following matrix multiplication (remember, transpose of a skew-symmetric matrix is the negative of the original matrix and the cross product is anti-commutative, i.e. a x b = -(b x a)).
(I have borrowed the big-R notation from Dyn4j)
and with the above, the velocity equation expressed as below:
can be expressed as:
From here, we can isolate the velocities easily to come up with:
where I is the identity matrix, not moment of inertia.
And, by inspection, we can say that the Jacobian matrix is as follows:
Calculating the K matrix
K matrix is given by the following equation:
which is:
Multiplying from left to right...
Multiplying left to right again, you end up with:
Now if we write out the equation element wise (remembering that inertia I is a scalar in 2D):
Multiplying the matrices and adding them yields:
Note that, unlike the contact constraint, K matrix is...actually a matrix (K matrix turned out to be a scalar in the contact constraint).
If you squint your eyes, you will see that the above is the same as the formula given in the Box2D-Lite docs, as follows:
where tilde (~) is explained as denoting the "cross-product matrix"
..and looking at the underlying C++ code shows how it has been implemented
(C++ Code)
// Pre-compute anchors, mass matrix, and bias.
Mat22 Rot1(body1->rotation);
Mat22 Rot2(body2->rotation);
r1 = Rot1 * localAnchor1;
r2 = Rot2 * localAnchor2;
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
Mat22 K1;
K1.col1.x = body1->invMass + body2->invMass; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = body1->invMass + body2->invMass;
Mat22 K2;
K2.col1.x = body1->invI * r1.y * r1.y; K2.col2.x = -body1->invI * r1.x * r1.y;
K2.col1.y = -body1->invI * r1.x * r1.y; K2.col2.y = body1->invI * r1.x * r1.x;
Mat22 K3;
K3.col1.x = body2->invI * r2.y * r2.y; K3.col2.x = -body2->invI * r2.x * r2.y;
K3.col1.y = -body2->invI * r2.x * r2.y; K3.col2.y = body2->invI * r2.x * r2.x;
Mat22 K = K1 + K2 + K3;
K.col1.x += softness;
K.col2.y += softness;
M = K.Invert();
The slightly confusing part of the equation is the bit referred to as the cross-product matrix above, and skew(r1)/skew(r2) below, since it is not immediately obvious how you create such matrices in 2D. However, let's just treat the 2D r1 and r2 vectors as 3D vectors with the z-component set to zero.
The code breaks down the K matrix into 3 parts as follows:
Taking a look at each of the terms:
and
and similarly:
Effective Mass
The K-matrix is then "inverted" to arrive at the effective mass and stored in this.M (don't confuse with the mass matrix).
Remember K is a 2x2 matrix and the inverse of a 2x2 matrix A,
is given by:
where (ad-bc) is the determinant of A and the matrix on the right-hand side is the Adjoint of A.
Javascript translation of the C++ 2x2 matrix invert method is as shown below.
invert() {
var B = new Mat22();
var a = this.col1.x;
var b = this.col2.x;
var c = this.col1.y;
var d = this.col2.y;
var det = (a * d - b * c);
assert(det !==0);
det = 1 / det;
B.col1.set(det * d, -det * c);
B.col2.set(-det * b, det * a)
return B;
}Bias Velocity and Impulse
Box2D-Lite docs explain as follows.
The error is the separation between the anchor points (x being the centre of mass).
and expresses the bias velocity as follows:
which is implemented as follows:
(C++Code)
Vec2 p1 = body1->position + r1;
Vec2 p2 = body2->position + r2;
Vec2 dp = p2 - p1;
if (World::positionCorrection)
{
bias = -biasFactor * inv_dt * dp;
}Note that the bias term must also be expressed as a vector (since the positional error is a vector), as opposed to a scalar as in the contact constraint (where the positional error was a scalar, ie the separation distance).
Be careful of the sign
As noted in the contact constraint example, a lot of tutorials will show the velocity bias equation without the minus sign, and the Lagrange multiplier as:
and the velocity delta is defined as:
such that:
Calculating the Lagrange Multiplier, Applying the Impulse
Note that the JV part of the Lagrange multiplier is the relative velocity at the anchor point, so the lambda expression (ie impulse magnitude) can be rewritten as:
λ=-(relative velocity+velocity bias) * EffectiveMass(Effective Mass is the inverse of the K-Matrix)
In the original C++ implementation, this looks straightforward enough, as follows (ignore the purple highlighted softness factor for this post - we'll come back to it in another post):
First the blue highlighted code dv is the relative velocity, and stored in dv.
Then the impulse is calculated by multiplying (bias - dv) by the effective mass (remember that the bias was calculated as the negative of the positional error). Again, it is important to note that the calculated impulse here is a vector, and not a scalar as was with the contact constraint.
Final line is accumulating the impulse. There is no complicated clamping of accumulated impulse and calculating the incremental impulse, as there was in contract constraint. However, accumulated impulse is necessary to implement the softness factor (which I will not discuss in this post)
ApplyImpulse()
{
Vec2 dv = body2->velocity + Cross(body2->angularVelocity, r2) - body1->velocity - Cross(body1->angularVelocity, r1);
Vec2 impulse;
impulse = M * (bias - dv - softness * P);
body1->velocity -= body1->invMass * impulse;
body1->angularVelocity -= body1->invI * Cross(r1, impulse);
body2->velocity += body2->invMass * impulse;
body2->angularVelocity += body2->invI * Cross(r2, impulse);
P += impulse;
}Javascript translation
The complete Javascript translation is shown below.
In particular, the ApplyImpulse is considerably longer and less easy to read than the C++ implementation because of the lack of overloading in Javascript.
class Joint {
constructor() {
this.body1;
this.body2;
this.M = new Mat22();
this.localAnchor1 = new Vector2();
this.localAnchor2 = new Vector2();
this.r1 = new Vector2();
this.r2 = new Vector2();
this.bias = new Vector2();
this.P = new Vector2();
this.softness = 0.0;
this.biasFactor = 0.2;
}
set(b1, b2, anchor) {
this.body1 = b1;
this.body2 = b2;
var Rot1 = new Mat22(this.body1.rotation);
var Rot2 = new Mat22(this.body2.rotation);
var Rot1T = Rot1.transpose();
var Rot2T = Rot2.transpose();
this.localAnchor1 = Mat22.MultMV(Rot1T, anchor.subi(this.body1.position));
this.localAnchor2 = Mat22.MultMV(Rot2T, anchor.subi(this.body2.position));
}
preStep(inv_dt) {
var body1 = this.body1;
var body2 = this.body2;
// Pre-compute anchors, mass matrix, and bias.
var Rot1 = new Mat22(this.body1.rotation);
var Rot2 = new Mat22(this.body2.rotation);
this.r1 = Mat22.MultMV(Rot1, this.localAnchor1);
this.r2 = Mat22.MultMV(Rot2, this.localAnchor2);
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
var K1 = new Mat22();
K1.col1.x = body1.invMass + body2.invMass; K1.col2.x = 0.0;
K1.col1.y = 0.0; K1.col2.y = body1.invMass + body2.invMass;
var K2 = new Mat22();
K2.col1.x = body1.invI * this.r1.y * this.r1.y; K2.col2.x = -body1.invI * this.r1.x * this.r1.y;
K2.col1.y = -body1.invI * this.r1.x * this.r1.y; K2.col2.y = body1.invI * this.r1.x * this.r1.x;
var K3 = new Mat22();
K3.col1.x = body2.invI * this.r2.y * this.r2.y; K3.col2.x = -body2.invI * this.r2.x * this.r2.y;
K3.col1.y = -body2.invI * this.r2.x * this.r2.y; K3.col2.y = body2.invI * this.r2.x * this.r2.x;
var K = K1.add(K2).add(K3);
K.col1.x += this.softness;
K.col2.y += this.softness;
this.M = K.invert();
var p1 = body1.position.addi(this.r1);
var p2 = body2.position.addi(this.r2);
var dp = p2.subi(p1);
if (world.positionCorrection)
{
// this.bias = Vector2.MulSV(-this.biasFactor, dp.mul(inv_dt));
this.bias = dp.scalei(inv_dt).scale(-this.biasFactor)
}
else
{
this.bias.set(0.0, 0.0);
}
if (world.warmStarting)
{
// Apply accumulated impulse.
this.body1.velocity = this.body1.velocity.subi(this.P.scalei(this.body1.invMass));
this.body1.angularVelocity -= this.body1.invI * Vector2.CrossVV(this.r1, this.P);
this.body2.velocity = this.body2.velocity.addi(this.P.scalei(this.body2.invMass));
this.body2.angularVelocity += this.body2.invI * Vector2.CrossVV(this.r2, this.P);
}
else
{
this.P.set(0.0, 0.0);
}
}
applyImpulse() {
var b1v = this.body1.velocity.addi(Vector2.CrossNV(this.body1.angularVelocity, this.r1));
var b2v = this.body2.velocity.addi(Vector2.CrossNV(this.body2.angularVelocity, this.r2));
var dv = b2v.subi(b1v);
var impulse = Mat22.MultMV(this.M, this.bias.subi(dv).subi(this.P.scalei(this.softness)) );
this.body1.velocity = this.body1.velocity.subi(impulse.scalei(this.body1.invMass)) ;
this.body1.angularVelocity -= this.body1.invI * Vector2.CrossVV(this.r1, impulse);
this.body2.velocity = this.body2.velocity.addi(impulse.scalei(this.body2.invMass));
this.body2.angularVelocity += this.body2.invI * Vector2.CrossVV(this.r2, impulse);
this.P = this.P.addi(impulse);
}
}