generalrelativity

In the previous article we implemented a particle constrained to a curve in two-dimensional space. We’ll expand on the concepts presented there and go through the process of creating modular constraints which can be easily interconnected. You’ll want to have some experience with 2D rigid body dynamics as we’ll be leaving behind the simplicity of particles for now. If “rigid body dynamics” sounds foreign to you, I recommend starting with the first 2 articles by Chris Hecker here.

As a fun little introduction to what we’ll be solving, look at the multi-body simulation below. The wheel has a torque applied and each body experiences a gravitational force. 1 ground constraint, 3 revolute joints and a prismatic joint constrain the system. Click the checkbox to toggle torque.

Demo: Roll over to simulate slider-crank.

The reason I wanted to write this article is because I found an imbalance between theory and implementation in the decent amount of freely available literature on the topic. The major rift exists, I think, because actual implementation involves radical optimizations that transform the tidy mathematical nature of the theory. In the concrete process and examples below, we’ll avoid this deviation with the hope that the material presented is a bit more accessible.

Global Form of the Constraint Problem

When we solved the particle-to-curve problem, it was sufficient to write the equations of motion in terms of the parabolic constraint. In the case that multiple constraints are acting upon a physical element, we need to write the equations of motion in terms of all constraints, because solving one may violate another. We could decide on a system configuration and solve each constraint force to derive the equations of motion, but that would be a lot of work; and not just once, but for each unique configuration.

Instead, we can construct a linear system of the form Ax = b, which can be solved for x generically. This allows us to piece together constraints anyway we like, which is a much more manageable way of setting up a simulation than sketching out the equations of motion for a giant, complex configuration. Now all we have to do is create this system, which basically just involves a lot of organization.

First, we need to compile the state of the system into a vector, q:

where n is the number of bodies. Each body is defined by an index, its position along the world x and y-axes and its orientation (?). Next, we compile all constraints imposed on the system into the global constraint vector, C:

where s is the number of constraint equations. Everything we need to construct the aforementioned linear system is either directly derived from q and C, or ordered to match these vectors. Now we’ll see what we’re really working toward (see Witkin for derivation):

Equation 1:

This looks a little scary at first, but it’s actually not that bad. M^-1 is a diagonal matrix of dimension 3n, where each element on the diagonal is the inverse intertial term associated with the element at the corresponding index in q:

Where m_k is the mass of body k and I_k is the moment of inertia about the center of mass of body k. Looking at q, we see that mass is associated with translation and moment of inertia is associated with rotation, which is what we know from the Newton-Euler equations.

F_ext is the global vector of all external forces and torques exerted on the bodies:

which is of dimension 3n, also. Here, ?„_k is the torque acting on body k. We could now write the unconstrained equations of motion as:

which should make sense of all the purposeful ordering we’ve seen so far.

Next, we’ll define the only radically new term introduced in Equation 1, J. This is known as the Jacobian of C and is an s-by-3n matrix, being composed of 1 row per constraint function (element of C) and 3 columns per body (x, y and ?). Row i is the gradient of the ith constraint function with respect to q:

Since a constraint generally involves at most 2 bodies, the gradient will be composed of mostly zeroes. The global Jacobian is:

This all might seem a bit abstract right now, but we’ll see that these terms are easy to construct in practice when we derive a few constraints. The time derivative of the Jacobian is built the same way, but with the velocity constraint vector:

The coefficients ks and kd of equation 1 are spring and drag terms, respectively, and used to correct error. Because each constraint equation should equal zero when satisfied, the amount of error is defined in the evaluation of the constraint functions.

Finally, we show how Equation 1 relates to the general form of a linear system mentioned above:

A is an s-by-s matrix, and x and b are vectors of dimension s. Solving for ? leads to the global constraint force vector:

Finally, the equations of motion for the constrained system can be written abstractly as:

Now we have a method for formulating constraints which can participate in the global system:

1. Write the positional constraint, C, as a function of q.
2. Take the derivative of C to solve for the velocity constraint.
3. Solve for the Jacobian of C.
4. Solve for the derivative of the Jacobian with respect to time.

Computer Implementation

public class Constraint
{
 
	//index in global constraint vector
	public var index:uint;
 
	//indicates the number of constraint functions
	//this is equal to the number of system DOF it removes
	internal function get numJacobianRows() : uint
	{
		//abstract
		return 0;
	}
 
	//populate the system matrices and vectors
	internal function solve( J:MatrixND, JDot:MatrixND, C:Vector.<Number> ) : void
	{
		//abstract
	}
 
}

Note that while we use C’ to solve for J’, we don’t actually need to explicitly contribute to the velocity constraint because C’ = Jq’ and can be solved at the global level without additional input from the individual constraints. Although this adds computational overhead, I like the generality achieved. Further, because we can alternatively solve for J’ by directly taking the derivative of J with respect to time, solving for C’ becomes seemingly unnecessary altogether. That said, I’ve found that the intuition-confirming statement a velocity constraint makes is worth the little bit of extra differentiation.

Now we’ll see how the global system is constructed and solved. Below is the relevant portion of the ConstraintSolver class, minus some declarations for conciseness’ sake. It allows each constraint to evaluate its very own special part of the structure via the above interface:

public class ConstraintSolver
{
 
	private var numJacobianRows:uint; //global # of constraint functions
	private var ode:DynamicsODE; 
 
	public function addConstraint( constraint:Constraint, initialize:Boolean = false ) : void
	{
		constraint.index = numJacobianRows;
		numJacobianRows += constraint.numJacobianRows;
		constraints.push( constraint );
		if( initialize ) initSystem();
	}
 
	public function initSystem() : void
	{
		W = new MatrixND( ode.length, ode.length, false );
		W.diagonal = ode.invMass;
		J = new MatrixND( numJacobianRows, W.m, false );
		JDot = new MatrixND( numJacobianRows, W.m, false );
		C = new Vector.<Number>( numJacobianRows, true );
		b = new Vector.<Number>( numJacobianRows, true );
		lambda = new Vector.<Number>( numJacobianRows, true );
	}
 
	public function solve() : void
	{
 
		for each( var constraint:Constraint in _constraints )
		{
			constraint.solve( J, JDot, C );
		}
 
		var JW:MatrixND = J.times( W );
		var JT:MatrixND = J.transpose;
		var JWJT:MatrixND = JW.times( JT );
 
		//J'q'
		var JDotqDot:Vector.<Number> = JDot.timesVector( ode.v );
		var JWFext:Vector.<Number> = JW.timesVector( ode.f );
 
		//C' = Jq'
		var CDot:Vector.<Number> = J.timesVector( ode.v );
 
		//b = -J'q' - JWQ - Cks - C'kd
		for( var i:int = 0; i < numJacobianRows; i++ )
		{
			b[ i ] = -JDotqDot[ i ] - JWFext[ i ] - C[ i ] * ks - CDot[ i ] * kd;
		}
 
		//solve the system
		JWJT.solveAnalytical( lambda, b );
 
		//constraint force = JTx
		var Fc:Vector.<Number> = JT.timesVector( lambda );
 
		//add the constraint force to the external forces
		for( i = 0; i < ode.length; i++ )
		{
			ode.f[ i ] += Fc[ i ];
		}
 
	}
 
}

With this structure in mind, we’ll now go through the process of deriving and implementing a couple constraints.

Ground Constraint

The first constraint we’ll derive with this method is the ground constraint, which keeps a body fixed in space. The body is not permitted to translate or rotate. First, we write the positional contraint:

where c_x is where the body is fixed along the global x-axis, c_y along the y, and c_? the fixed orientation of the body. Note that 0 is the zero vector, [0,0,0]. Next, we solve the velocity constraint by taking the derivative of C_ground with respect to time:

which simply and intuitively states that linear and angular velocity are zero. This is what we expect considering the constraint prevents the body from experiencing any motion. Next, we derive J:

And, finally, the derivative of J w.r.t. time:

That was not so bad at all! The Jacobian can be a bit off-putting at times and make the problem seem more complicated than it is–just remember that we’re simply creating a matrix where each row is the gradient of a constraint function. In practice, you wouldn’t want to simulate the ground constraint, but it’s a good exercise because of how simple the math is. Other joints will have more complex positional constraints, but the underlying framework for formulating these quantities is the same.

The GroundConstraint class:

public class GroundConstraint extends Constraint
{
 
	public var body:Body2D;
 
	private var U:Point;
	private var theta:Number;
 
	public function GroundConstraint( body:Body2D )
	{
		super();
 
		this.body = body;
 
		U = body.position.clone();
		theta = body.theta;
 
	}
 
	override internal function get numJacobianRows() : uint
	{
		//removes all DOF
		return 3;
	}
 
	override internal function solve( J:MatrixND, JDot:MatrixND, C:Vector.<Number> ) : void
	{
 
		J.A[ index ][ body.index 	] = 1;
		J.A[ index ][ body.index + 1 	] = 0;
		J.A[ index ][ body.index + 2 	] = 0;
 
		J.A[ index + 1 ][ body.index 		] = 0;
		J.A[ index + 1 ][ body.index + 1 	] = 1;
		J.A[ index + 1 ][ body.index + 2 	] = 0;
 
		J.A[ index + 2 ][ body.index 		] = 0;
		J.A[ index + 2 ][ body.index + 1 	] = 0;
		J.A[ index + 2 ][ body.index + 2 	] = 1;
 
		JDot.A[ index ][ body.index 		] = 0;
		JDot.A[ index ][ body.index + 1 	] = 0;
		JDot.A[ index ][ body.index + 2 	] = 0;
 
		JDot.A[ index + 1 ][ body.index 	] = 0;
		JDot.A[ index + 1 ][ body.index + 1 	] = 0;
		JDot.A[ index + 1 ][ body.index + 2 	] = 0;
 
		JDot.A[ index + 2 ][ body.index 	] = 0;
		JDot.A[ index + 2 ][ body.index + 1 	] = 0;
		JDot.A[ index + 2 ][ body.index + 2 	] = 0;
 
		//positional error
		C[ index 	] = body.x - U.x;
		C[ index + 1 	] = body.y - U.y;
		C[ index + 2 	] = body.theta - theta;
 
	}
 
}

Here we finally see how the indexing of the constraint and the body play into the global vectors and matrices.

Revolute Joint

A revolute joint, in two dimensions, is like paper pinned up on a wall. The paper is prevented from falling to the ground, but can spin freely. To formulate the joint constraint, we need a mathematical representation of a point on a rigid body.

Point on Rigid Body

First, we’ll define the terms used to describe rigid body state. Just as with a particle, x is the body’s position vector:

A 2D body is also characterized by its orientation about the axis poking out of the screen. This is represented as the 2D rotation matrix, A:

Which is clearly parametrized by the angle, ?. Given these representations, we can solve for the world-space position of any point, P, that is defined in the body’s local coordinate frame. We designate this term r:

Equation 2:

Where u is the vector pointing from the body’s origin to P. Equation 2 can be decomposed from vector form into 2 scalar equations:

Figure 1: Rigid Body Transfromation

Figure 1 shows a rigid body’s local coordinate system and that same system transformed into world-space. Note that in the body’s local coordinate frame, all points and vectors are constants, whereas in the global frame, they are functions of time. Equation 2 can be written more explicitly to reflect this:

Although we’ll prefer the abbreviated form of equation 2, it’s important to remember which terms are functions of time, as we differentiate this function below.

Figure 2: Revolute Joint

The revolute joint pins 1 body to another at a shared point in world space. Because we can represent this point in terms of each body as shown in equation 2, the positional constraint is easy to write:

which states that the distance between the 2 bodies at these respective points is zero. Differentiating with respect to time leads to the velocity constraint:

indicating that the relative velocity of the bodies at these points is zero. Next, we evaluate the Jacobian of C_revolute:

And, finally, the time derivative of the Jacobian:

The RevoluteJoint class:

public class RevoluteJoint extends Constraint implements IRenderable
{
 
	public var b1:Body2D;
	public var b2:Body2D;
 
	private var u1:Point;
	private var u2:Point;
 
	public function RevoluteJoint( b1:Body2D, b2:Body2D, U:Point )
	{
 
		super();
 
		this.b1 = b1;
		this.b2 = b2;
 
                //convert U to b1's local coordinate frame
		var T:Matrix = b1.A.clone();
		T.invert();
		u1 = T.transformPoint( U.subtract( b1.position ) );
 
                //convert U to b2's local coordinate frame
		T = b2.A.clone();
		T.invert();
		u2 = T.transformPoint( U.subtract( b2.position ) );
 
	}
 
	override internal function get numJacobianRows() : uint
	{
		//removes 2 DOF
		return 2;
	}
 
	override internal function solve( J:MatrixND, JDot:MatrixND, C:Vector.<Number> ) : void
	{
 
		var uxc1:Number = u1.x * b1.A.a;	var uxc2:Number = u2.x * b2.A.a;
		var uyc1:Number = u1.y * b1.A.a;	var uyc2:Number = u2.y * b2.A.a;
		var uxs1:Number = u1.x * b1.A.b;	var uxs2:Number = u2.x * b2.A.b;
		var uys1:Number = u1.y * b1.A.b;	var uys2:Number = u2.y * b2.A.b;
 
		//J
		J.A[ index ][ b1.index 		] = 1;
		J.A[ index ][ b1.index + 1 	] = 0;
		J.A[ index ][ b1.index + 2 	] = -uyc1 - uxs1;
 
		J.A[ index + 1 ][ b1.index 		] = 0;
		J.A[ index + 1 ][ b1.index + 1 	] = 1;
		J.A[ index + 1 ][ b1.index + 2 	] = uxc1 - uys1;
 
		J.A[ index ][ b2.index 		] = -1;
		J.A[ index ][ b2.index + 1 	] = 0;
		J.A[ index ][ b2.index + 2 	] = uyc2 + uxs2;
 
		J.A[ index + 1 ][ b2.index 		] = 0;
		J.A[ index + 1 ][ b2.index + 1 	] = -1;
		J.A[ index + 1 ][ b2.index + 2 	] = -uxc2 + uys2;
 
		//J'
		JDot.A[ index ][ b1.index 		] = 0;
		JDot.A[ index ][ b1.index + 1 	] = 0;
		JDot.A[ index ][ b1.index + 2 	] = b1.av * ( -uxc1 + uys1 );
 
		JDot.A[ index + 1 ][ b1.index 	] = 0;
		JDot.A[ index + 1 ][ b1.index + 1 	] = 0;
		JDot.A[ index + 1 ][ b1.index + 2 	] = b1.av * ( -uyc1 - uxs1 );
 
		JDot.A[ index ][ b2.index 	] = 0;
		JDot.A[ index ][ b2.index + 1 	] = 0;
		JDot.A[ index ][ b2.index + 2 	] = b2.av * ( uxc2 - uys2 );
 
		JDot.A[ index + 1 ][ b2.index 	] = 0;
		JDot.A[ index + 1 ][ b2.index + 1 	] = 0;
		JDot.A[ index + 1 ][ b2.index + 2 	] = b2.av * ( uyc2 + uxs2 );
 
		//positional error
		C[ index 	] = b1.x + uxc1 - uys1 - b2.x - uxc2 + uys2;
		C[ index + 1 	] = b1.y + uxs1 + uyc1 - b2.y - uxs2 - uyc2;
 
	}
 
}

Considerations

We’ve followed the mathematical formulas closely, which has left us with a computationally inefficient application. Hopefully this trade has been for a solid understanding that lays the foundation for the reader to implement a system of constraints. As a next step, I recommend reading Erin Catto’s paper, Iterative Dynamics with Temporal Coherence, which highlights several optimizations in both time and memory.

Just as was the case with the particle-to-curve simulation, an RK4 integrator was necessary to not accumulate an insurmountable amount of error. The choice of the spring and drag coefficients becomes less trivial than expected and aid in the degradation of accuracy.

In later articles, we’ll switch from acceleration to a velocity-based constraint solver. This is simpler, more efficient and capable of handling a wider variety of constraint problems.