Edit: There’s an error in this page! The position constraint should have been defined in terms of dot products. It’s a small math mistake, and I didn’t notice it since during implementation I used dot products in place of actual angles.

I decided today that I wanted to create an orientation constraint. This will let me build more complex joints that make use of constraining relative angles between rigid bodies. I learned the details of implementing such a constraint from Bart Barenbrug’s thesis.

The position constraint is quite simple. The idea is to take a basis of vectors (u, v, w) into local space of two rigid bodies A and B. This yields xa, xb, ya, yb, za and zb. Each frame the local basis vectors are transformed into world space. These two world space sets should be aligned at all times in order to satisfy the constraint.

The dot product of xa and yb provides information about the error along u. The dot product of xa and zb provides information about the error along v. The dot product of ya and zb provides information about the error along w. Knowing this four vectors can be used instead of six, as the only ones needed are: xa, ya, yb, zb.

\begin{equation}

C = \theta_a – \theta_b = 0

\label{eq1}

\end{equation}

Deriving the equation yields:

\begin{equation}

\dot{C} = \omega_a – \omega_b = 0

\label{eq2}

\end{equation}

Resulting in the following Jacobian:

\begin{equation}

J = \begin{bmatrix} 0 & -t & 0 & t \end{bmatrix}

\label{eq3}

\end{equation}

Where *t* is an axis of rotation within a basis matrix. Three of these constraints can be used to restrict each axis of rotation.

The interesting part of this constraint, for myself at least, was writing down on paper the mass matrix used to solve all three constraints simultaneously. All three constraints meaning restricting the relative rotation between two rigid bodies, aka the orientation joint (or angle joint). As a quick refresher the mass matrix is within the following equation:

\begin{equation}

\lambda = \frac{-(JV + B)}{J * M^{-1} * J^T}

\label{eq4}

\end{equation}

The bottom portion *J * M ^{-1} * J^{T}* would be the constraint mass. This is actually a tensor and must be transformed from constraint space by using the tensor transformation formula. This is the same sort of transformation used on inertia tensors to transform them from model to world space.

Once this mass matrix is calculated it can be inverted and used to solve for lambda, where lambda will satisfy all constraint equations used in the mass matrix.

\begin{equation}

M^{-1} = \begin{bmatrix}

M^{-1}_a & 0 & 0 & 0 \\

0 & I^{-1}_a & 0 & 0 \\

0 & 0 & M^{-1}_b & 0 \\

0 & 0 & 0 & I^{-1}_b

\end{bmatrix}

\label{eq5}

\end{equation}

Knowing this we can setup our constraint mass like so:

\begin{equation}

\begin{bmatrix}

0 & -t_1 & 0 & t_1 \\

0 & -t_2 & 0 & t_2 \\

0 & -t_3 & 0 & t_3 \\

\end{bmatrix}

\begin{bmatrix}

M^{-1}_a & 0 & 0 & 0 \\

0 & I^{-1}_a & 0 & 0 \\

0 & 0 & M^{-1}_b & 0 \\

0 & 0 & 0 & I^{-1}_b

\end{bmatrix}

\begin{bmatrix}

0 & 0 & 0 \\

-t_1 & -t_2 & -t_3 \\

0 & 0 & 0 \\

t_1 & t_2 & t_3 \\

\end{bmatrix}

\label{eq6}

\end{equation}

Multiplying the two equations on the left gives:

\begin{equation}

\begin{bmatrix}

0 & I^{-1}_a * -t_1 & 0 & I^{-1}_b * t_1 \\

0 & I^{-1}_a * -t_2 & 0 & I^{-1}_b * t_2 \\

0 & I^{-1}_a * -t_3 & 0 & I^{-1}_b * t_3 \\

\end{bmatrix}

\begin{bmatrix}

0 & 0 & 0 \\

-t_1 & -t_2 & -t_3 \\

0 & 0 & 0 \\

t_1 & t_2 & t_3 \\

\end{bmatrix}

\label{eq7}

\end{equation}

Multiplying the last two equations leaves the final constraint mass matrix:

\begin{equation}

\begin{bmatrix}

\scriptsize -t_1 \cdot I^{-1}_a -t_1 + t_1 \cdot I^{-1}_b t_1 &

\scriptsize -t_2 \cdot I^{-1}_a -t_1 + t_2 \cdot I^{-1}_b t_1 &

\scriptsize -t_3 \cdot I^{-1}_a -t_1 + t_3 \cdot I^{-1}_b t_1 \\

\scriptsize -t_1 \cdot I^{-1}_a -t_2 + t_1 \cdot I^{-1}_b t_2 &

\scriptsize -t_2 \cdot I^{-1}_a -t_2 + t_2 \cdot I^{-1}_b t_2 &

\scriptsize -t_3 \cdot I^{-1}_a -t_2 + t_3 \cdot I^{-1}_b t_2 \\

\scriptsize -t_1 \cdot I^{-1}_a -t_3 + t_1 \cdot I^{-1}_b t_3 &

\scriptsize -t_2 \cdot I^{-1}_a -t_3 + t_2 \cdot I^{-1}_b t_3 &

\scriptsize -t_3 \cdot I^{-1}_a -t_3 + t_3 \cdot I^{-1}_b t_3 \\

\end{bmatrix}

\label{eq8}

\end{equation}

I apologize for the tiny text! Using this mass matrix like the following can be done to implement an orientation constraint to restrict the relative rotation of two rigid bodies:

1 2 3 4 5 6 7 8 9 10 |
// M is the constraint mass M = M.Inverse( ); Vec3 lambda = M * -(JV + b); // Implement lambda * JV // Apply impulse on body A and B // t1, t2 and t3 are basis to align to (the Jacobian elements) Vec3 P = t1 * lambda.x + t2 * lambda.y + t3 * lambda.z; A->angularVel -= iA * P; B->angularVel += iB * P; |