I am looking for a general formulation to find the closest points on two line segments. What I was thinking about is to define our lines as:
$$ P1 + s (P2-P1)$$
$$ Q1 + t (Q2-Q1)$$
Where $P1 , P2, Q1$ and $Q2$ are the beginning and the end points on each segment.
Now we should go through an optimization problem as: $\min f(s,t)$ such that $0<s<1$ and $0<t<1$. Where $f(s,t)$ is the point-to-point distance function.
Is there any straight forward solution?
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))$$
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:
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
/!\ In the post by @nominal-animal there are sign errors in this presentation (at least leading up to the calculation of s and t). The correct s and t are given by the following Python routine that uses SymPy:
def close_points(l1, l2):
"""return s and t, the parametric location of the closest
points on l1 and l2, the actual points, and the separation between
those two points.
NOTE: these were checked on wolframalpha.com with query, e.g.
"closest points on Line((0,0,0),(1,1,1)) and Line((0,1,0),(1,2,3))"
Examples
========
>>> from sympy import Line
>>> close_points(Line((0, 0, 0), (1, 1, 1)), Line((0, 1, 0), (1, 2, 3)))
(3/4, 1/4, Point3D(3/4, 3/4, 3/4), Point3D(1/4, 5/4, 3/4), sqrt(2)/2)
>>> close_points(Line((0, 0, 0), (1, 2, 3)), Line((1, 1, 1), (2, 3, 5)))
(7/5, 4/5, Point3D(7/5, 14/5, 21/5), Point3D(9/5, 13/5, 21/5), sqrt(5)/5)
"""
# L1 = lambda s: l1.p1+s*l1.direction
# L2 = lambda t: l2.p1+t*l2.direction
# eq = lambda l1: Matrix(L1(var('s')) - L2(var('t'))).dot(l1.direction)
# stdict = solve((
# eq(Line(var('x1 y1 z1'),var('x2 y2 z2'))),
# eq(Line(var('x3 y3 z3'),var('x4 y4 z4')))),
# var('s t'), dict=True)
x1,y1,z1 = l1.p1
x2,y2,z2 = l1.p2
x3,y3,z3 = l2.p1
x4,y4,z4 = l2.p2
x21, y21, z21 = l1.direction
x43, y43, z43 = l2.direction
x31, y31, z31 = l2.p1 - l1.p1
R1 = x21**2 + y21**2 + z21**2
R2 = x43**2 + y43**2 + z43**2
d4321 = x21*x43 + y21*y43 + z21*z43
d3121 = x21*x31 + y21*y31 + z21*z31
d4331 = x31*x43 + y31*y43 + z31*z43
den = d4321**2 - R1*R2
# R1*s - d4321*t - d3121 = 0
# d4321*s - R2*t - d4331 = 0
s = (d4321*d4331 - R2*d3121)/den
t = (R1*d4331 - d4321*d3121)/den
ps = l1.p1 + s*l1.direction
pt = l2.p1 + t*l2.direction
return s, t, ps, pt, ps.distance(pt)
