Closest points on two line segments
Solution 1:
Since the thing you are minimizing is an everywhere-positive quadratic function of $t$ and $s$, it is convex in each variable. So, we need its critical point, and failing that, something close to it.
The function has a critical point when the vector connecting points on two lines is orthogonal to each line. At this point, the vector $$v = P1 + s(P2-P1) - Q1- t(Q2−Q1)$$ satisfies $$v\perp (P2-P1) \quad \text{ and} \quad v\perp (Q2-Q1)$$ This is a system of two linear equations with two unknowns $s,t$. Having solved it, you may find that either:
- Both $t$ and $s$ are between $0$ and $1$. Then they give the minimum
- One or both of $t,s$ are outside of the interval $[0,1]$. Then replace the outlying number with the nearest point of the interval $[0,1]$.
Solution 2:
Because there are multiple sub-cases, and the existing answer only outlines the procedure, I wanted to describe the entire procedure.
Let's assume our first line segment $L_1$ is $$\vec{p}_1 = \left [ \begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix} \right ], \quad \vec{p}_2 = \left [ \begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix} \right ], \quad L_1(s) = \vec{p}_1 + s ( \vec{p}_2 - \vec{p}_1 ) = (1-s)\vec{p}_1 + s \vec{p}_2 \tag{1}\label{NA1}$$ and similarly the second line segment $L_2$ is $$\vec{p}_3 = \left [ \begin{matrix} x_3 \\ y_3 \\ z_3 \end{matrix} \right ], \quad \vec{p}_4 = \left [ \begin{matrix} x_4 \\ y_4 \\ z_4 \end{matrix} \right ], \quad L_2(t) = \vec{p}_3 + t ( \vec{p}_4 - \vec{p}_3 ) = (1-t)\vec{p}_3 + t \vec{p}_4 \tag{2}\label{NA2}$$ Note that we parametrize the line segments so that $L_1(0) = \vec{p}_1$, $L_1(1) = \vec{p}_2$, $L_2(0) = \vec{p}_3$, and $L_2(1) = \vec{p}_4$. I will use term "line segment" whenever we are restricted to the range $0 \le s \le 1$ or $0 \le t \le 1$, and "line" when we consider all $s \in \mathbb{R}$ or $t \in \mathbb{R}$.
The closest points on the two lines occur when the line connecting the two points is perpendicular to both lines: $$\left\lbrace \begin{aligned} (L_1(s) - L_2(t)) \cdot (\vec{p}_2 - \vec{p}_1) &= 0 \\ (L_1(s) - L_2(t)) \cdot (\vec{p}_4 - \vec{p}_3) &= 0 \\ \end{aligned} \right. \tag{3a}\label{NA3a}$$ which simplifies to $$\left\lbrace \begin{aligned} \bigl( (x_2 - x_1)^2 + (y_2 - y_1)^2 + (z_2 - z_1)^2 \bigr) s \; & + \\ -\bigl( (x_4 - x_3)(x_2 - x_1) + (y_4 - y_3)(y_2 - y_1) + (z_4 - z_3)(z_2 -z_1) \bigr) t \; & + \\ -\bigl( (x_3 - x_1)(x_2 - x_1) + (y_3 - y_1)(y_2 - y_1) + (z_3 - z_1)(z_2 - z_1) \bigr) & = 0 \\ \bigl( (x_4 - x_3)(x_2 - x_1) + (y_4 - y_3)(y_2 - y_1) + (z_4 - z_3)(z_2 - z_1) \bigr) s \; & + \\ -\bigl( (x_4 - x_3)^2 + (y_4 - y_3)^2 + (z_4 - z_3)^2 \bigr) t \; & + \\ -\bigl( (x_4 - x_3)(x_3 - x_1) + (y_4 - y_3)(y_3 - y_1) + (z_4 - z_3)(z_3 - z_1) \bigr) & = 0 \\ \end{aligned} \right. \tag{3b}\label{NA3b}$$ This has a single solution.
If we use temporary variables $$\begin{aligned} R_1^2 &= (x_2 - x_1)^2 + (y_2 - y_1)^2 + (z_2 - z_1)^2 \\ R_2^2 &= (x_4 - x_3)^2 + (y_4 - y_3)^2 + (z_4 - z_3)^2 \\ D_{4321} &= (x_4 - x_3)(x_2 - x_1) + (y_4 - y_3)(y_2 - y_1) + (z_4 - z_3)(z_2 - z_1) \\ D_{3121} &= (x_3 - x_1)(x_2 - x_1) + (y_3 - y_1)(y_2 - y_1) + (z_3 - z_1)(z_2 - z_1) \\ D_{4331} &= (x_4 - x_3)(x_3 - x_1) + (y_4 - y_3)(y_3 - y_1) + (z_4 - z_3)(z_3 - z_1) \\ \end{aligned}$$ we can write the system of equations as $$\left\lbrace\begin{aligned} R_1^2 s + D_{4321} t - D_{3121} &= 0 \\ D_{4321} s - R_2^2 t - D_{4331} &= 0 \\ \end{aligned}\right.$$ and the solution as $$\left\lbrace\begin{aligned} s &= \frac{D_{4321} D_{4331} + D_{3121} R_2^2}{R_1^2 R_2^2 + D_{4321}^2} \\ t &= \frac{D_{4321} D_{3121} - D_{4331} R_1^2}{R_1^2 R_2^2 + D_{4321}^2} \\ \end{aligned}\right.\tag{4}\label{NA4}$$ At this point, it is important to remember that $s$ and $t$ identify the closest points on the lines.
If, and only if $0 \le s \le 1$ and $0 \le t \le 1$, then $L_1(s)$ and $L_2(t)$ are the two closest points on the line segments $L_1$ and $L_2$, respectively.
Otherwise, we need to find the closest point on line segment $L_2$ to $\vec{p}_1$, the closest point on line segment $L_2$ to $\vec{p}_2$, the closest point on line segment $L_1$ to $\vec{p}_3$, and the closest point on line segment $L_1$ to $\vec{p}_4$, calculate their distances (or distances squared), and pick the pair with the smallest distance (squared).
Line $L_1$ is closest to point $\vec{p}_3$ at $L_1(s)$, when the line between the two points, $(L_1(s) - \vec{p}_3)$, is perpendicular to line $L_1$: $$(L_1(s) - \vec{p}_3)\cdot(\vec{p}_2 - \vec{p}_1) = 0$$ i.e. $$(\vec{p}_1 + s (\vec{p}_2 - \vec{p}_1) - \vec{p}_3)\cdot(\vec{p}_2 - \vec{p}_1) = 0$$ which simplifies to $$(\vec{p}_2 - \vec{p}_1)\cdot(\vec{p}_2 - \vec{p}_1) s - (\vec{p}_3 - \vec{p}_1)\cdot(\vec{p}_2 - \vec{p}_1) = 0$$ and is a linear equation, and trivial to solve (by moving the negative part to the right side, and dividing both sides by the multiplier of $s$).
If we use the following constants: $$\begin{aligned} D_{2121} = R_1^2 &= (\vec{p}_2 - \vec{p}_1)\cdot(\vec{p}_2 - \vec{p}_1) \\ D_{4343} = R_2^2 &= (\vec{p}_4 - \vec{p}_3)\cdot(\vec{p}_4 - \vec{p}_3) \\ D_{3121} &= (\vec{p}_3 - \vec{p}_1)\cdot(\vec{p}_2 - \vec{p}_1) \\ D_{4121} &= (\vec{p}_4 - \vec{p}_1)\cdot(\vec{p}_2 - \vec{p}_1) \\ D_{4321} &= (\vec{p}_4 - \vec{p}_3)\cdot(\vec{p}_2 - \vec{p}_1) \\ D_{4331} &= (\vec{p}_4 - \vec{p}_3)\cdot(\vec{p}_3 - \vec{p}_1) \\ D_{4332} &= (\vec{p}_4 - \vec{p}_3)\cdot(\vec{p}_3 - \vec{p}_2) \\ \end{aligned} \tag{5}\label{NA5}$$ (which are equivalent to the ones defined earlier; I just wanted to show how much more concise the vector algebra notation is), we can write the four point pairs as follows:
The closest point on line $L_1(s)$ to point $\vec{p}_3$ ($ = L_2(0)$) is at $s = \frac{d_{3121}}{R_1^2}$
The closest point on line $L_1(s)$ to point $\vec{p}_4$ ($ = L_2(1)$) is at $s = \frac{d_{4121}}{R_1^2}$
The closest point on line $L_2(t)$ to point $\vec{p}_1$ ($ = L_1(0)$) is at $t = \frac{-d_{4331}}{R_2^2}$
The closest point on line $L_2(t)$ to point $\vec{p}_2$ ($ = L_1(1)$) is at $t = \frac{-d_{4332}}{R_2^2}$
Do remember to clamp $0 \le s \le 1$ or $0 \le t \le 1$ before calculating the distance (squared) and choosing the pair with the smallest distance (squared), so that you find the point closest on the line segment, and not on the entire line.
To calculate the distance squared between say point $\vec{p}_2$ and point $L_2(t)$, use $$(\vec{p}_2 - L_2(t)) \cdot (\vec{p}_2 - L_2(t))$$
Solution 3:
Here's an implementation using NumPy
def closest_line_seg_line_seg(p1, p2, p3, p4):
P1 = p1
P2 = p3
V1 = p2 - p1
V2 = p4 - p3
V21 = P2 - P1
v22 = np.dot(V2, V2)
v11 = np.dot(V1, V1)
v21 = np.dot(V2, V1)
v21_1 = np.dot(V21, V1)
v21_2 = np.dot(V21, V2)
denom = v21 * v21 - v22 * v11
if np.isclose(denom, 0.):
s = 0.
t = (v11 * s - v21_1) / v21
else:
s = (v21_2 * v21 - v22 * v21_1) / denom
t = (-v21_1 * v21 + v11 * v21_2) / denom
s = max(min(s, 1.), 0.)
t = max(min(t, 1.), 0.)
p_a = P1 + s * V1
p_b = P2 + t * V2
return p_a, p_b
