Distance Constraint Derivation

Sometimes it’s desired to have a distance constraint. That constraint assumes a choosen separation distance for two points fixed in two bodies. I’ve written a derivation for it in 3D and shared here. An implementation should be pretty simple since we have minimally one Jacobian and solve a 1D linear system. Here is an example which you can use in your own projects.

Appendix

The boring part is that it is possible to have a non-defined gradient when this constraint is satisfied. However, the problem is solved by skipping the constraint solving when that happens. Another workaround I can recommend is to define 3 distance constraints and solve them as a 3-by-3 linear system. This is sometimes called a block solver. Separating the points is done by chosing a point outside the volume of the shape of one of the bodies. Here, fortunately, there are neither square roots nor divide-by-zero checks which generally yields in greater performance.

Note that an inertia tensor I is a symmetric 3-by-3 matrix and so do its inverse. Therefore, I = I^T. This simplifies the effective mass for this constraint (and can be applied to many other constraints) to

JM^{-1}J^T = m_1 + m_2 + (I^{-1}_1 A_1)^T A_1 + (I^{-1}_2 A_2)^T A_2

Another way to compute the velocity constraint vector is to build the gradients of the position constraint with respect to the position vector directly (Jacobian) in a piece of paper. Remember that if C(x(t))) then by the chain rule we have:

frac{ partial C } { partial t } = frac{ partial C } { partial x } frac{ partial x } { partial t } = Jv

Talking about the Jacobian, we can differentiate the velocity constraint to get the acceleration constraint. For that we can use a product rule-chain rule combo. By the product rule,

frac{d}{dt} (frac{partial C}{partial x}frac{partial x}{partial t}) = frac{d}{dt}(frac{partial C}{partial x}) frac{dx}{dt} + frac{partial C}{partial x} frac{d}{dt}(frac{dx}{dt}) = frac{d}{dt}(J) v + J a

where a = frac{dv}{dt}. By the chain rule,

frac{d}{dt}(frac{partial C}{partial x}) = frac{partial }{partial x}(frac{partial C}{partial x})frac{dx}{dt} = frac{partial }{partial x}(J)v

Usually it’s not required doing such a workload but I think it is a good vector calculus exercise.

Example

Let us consider the linear motion of two points x_1 and x_2. To keep things down to earth we use block matrices again. Let’s define

x in mathbb { R }^ { 6 times 1 } = begin{bmatrix} x_1 &x_2 end{bmatrix}^T

v in mathbb { R }^ { 6 times 1 } = begin{bmatrix} v_1 &v_2 end{bmatrix}^T

That is, the position and the velocity vector, respecively. Then, the vector stacking the 3 distance constraints that keep the points together at anytime is

C(x) : mathbb { R }^ { 6 times 1 } rightarrow mathbb { R }^ { 3 times 1 }

C(x) = x_2 - x_1

The Jacobian is then

frac{ partial C } { partial x } in mathbb { R }^ { 3 times 6 } = begin{bmatrix} frac{ partial C_1 } { partial x_1 } &frac{ partial C_1 } { partial x_2 } end{bmatrix}^T = begin{bmatrix} -I &I end{bmatrix}

where I is the identity matrix. Note that the Jacobian is just a vector of gradients in block form. From those we can compute the velocity constraint vector easely.

frac{ partial C } { partial t } = begin{bmatrix} -I &I end{bmatrix} v = -v_1 + v_2 = v_2 - v_1

We can see now that building the Jacobian by inspection is more painless, and predict that can be specially in the presence of angular motion. 

 

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s