# 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.