Point-Line Distances

A common problem in robotics and computer graphics is calculating the proximity between objects, and the closest points of collision. This page contains some useful equations for the closest points between points and line segments, building up to the closest points between two line segments.

For each step along the way there is a 3D demo, a mathematical derivation, and some C++ish example code.

tip

Don't worry if the equations seem a bit full-on, you'll see from the code examples that it's mostly pretty simple!

Note that while the demos are in 3D, all of these equations work exactly the same for 2D and 3D.

Here's a preview of the closest points between two line segments:

Some inspiration taken from this page.

Background & Assumptions

Below are the building blocks (both mathematical and code) that will be used in the rest of the page.

Geometric definition of dot product

$$\begin{align*} a \cdot b &= |a||b|\cos\theta \\ \frac{a \cdot b} {|b|} &= |a| \cos \theta \end{align*}$$

Vector Length

$$\begin{align*} |a| &= \sqrt{a \cdot a} \\ |a|^2 &= a \cdot a \end{align*}$$

Vector classes

Assume you have vector/point classes such as Vector2 or Vector3 that have x/y/(z) fields and handle your arithmetic.

Dot product

Assume you have a function for it, e.g. dot(a,b) = a.x*.x + a.y*b.y + a.z*b.z. If you are using a library this will usually come with the vector/point classes.

Interpolation

Interpolation is the process of selecting a point between two other points, often used for selecting a more appropriate value for a data point between discrete samples. One of the simplest interpolation methods is linear interpolation where the mixture of the two sample points is directly proportional to how far we have moved between them.

We use a variable $t$, where $t=0$ is the first point, $t=1$ is the second point, and anywhere in between represents the fraction moved between the points. The value can also extend beyond the bounds: $t < 0$ will be before the first point and $t > 1$ will be past the second (when we do this it's technically called linear extrapolation).

One simple way to implement linear interpolation is:

lerp(a,b,t) = a + t*(b-a)

Clamping

To clamp a value between two bounds e.g. 0 and 1:

clamp01(a) = min(max(a,0),1)

Division by 0

Whenever we are dividing, we need to make sure our denominator is not 0 (or very small). The easiest way is to check if the absolute value is smaller than some very small number that we call epsilon.

nearzero(val) = abs(val) < epsilon

Choosing an appropriate value for epsilon is a complex topic and will depend on the kind of problems you're solving - the examples below are using 0.000001.

Projecting a vector onto a vector

The key tool we will use over and over in these equations is projecting one vector onto another:

Let's say we have two vectors, $\yellow{\mathbf{u}}$ and $\purple{\mathbf{v}}$. We want to project $\yellow{\mathbf{u}}$ onto $\purple{\mathbf{v}}$ to make $\blue{\mathbf{u}'}$ - it's like $\yellow{\mathbf{u}}$'s shadow.

To construct $\blue{\mathbf{u}'}$ we first come up with an equation for its length using trigonometry, and then substitute the geometric equation for the dot product.

$$\begin{align*} |\blue{\mathbf{u}'}| &= |\yellow{\mathbf{u}}|\cos\theta \\ &= \frac{\yellow{\mathbf{u}} \cdot \purple{\mathbf{v}}}{|\purple{\mathbf{v}}|} \end{align*}$$

We know that $\blue{\mathbf{u}'}$ lies in the same direction as $\mathbf{\purple{v}}$, so we can recover the whole vector by multiplying its length by the unit vector $\green{\hat{\mathbf{v}}}$. The full derivation becomes:

$$\begin{align*} \text{proj}_\mathbf{\purple{v}} \yellow{\mathbf{u}} &= \blue{\mathbf{u}'} \\ &= |\blue{\mathbf{u}'}| \green{\hat{\mathbf{v}}} \\ &= \frac{\yellow{\mathbf{u}} \cdot \purple{\mathbf{v}}}{|\purple{\mathbf{v}}|} \frac{\purple{\mathbf{v}}}{|\purple{\mathbf{v}}|} \\ &= \frac{\yellow{\mathbf{u}} \cdot \purple{\mathbf{v}}}{\purple{\mathbf{v}} \cdot \purple{\mathbf{v}}} \purple{\mathbf{v}} \end{align*}$$

We can see that $\blue{\mathbf{u}'}$ is simply a scaled version of $\purple{\mathbf{v}}$, where the scaling factor is $\frac{\mathbf{u} \cdot \mathbf{v}}{\mathbf{v} \cdot \mathbf{v}}$. For simplicity we'll refer to this as $t$ in the following sections, such that:

$$\blue{\mathbf{u}'} = t \purple{\mathbf{v}} \\ t = \frac{\yellow{\mathbf{u}} \cdot \purple{\mathbf{v}}}{\purple{\mathbf{v}} \cdot \purple{\mathbf{v}}}$$

It's important to understand that when $0<t<1$, $\blue{\mathbf{u}'}$ lies inside the original $\purple{\mathbf{v}}$. When $t>1$, $\blue{\mathbf{u}'}$ grows longer than $\purple{\mathbf{v}}$, and when $t<0$ it begins to extend in the opposite direction to $\purple{\mathbf{v}}$.

warning

Remember to check your denominator is valid so you don't divide by 0 - this implies that $\mathbf{v}$ has no length or direction! What you want to do in this case depends on the particular problem you are solving.

Example Code

Vector ProjectVectorOntoVector(Vector u, Vector v)
{
    double denominator = v.dot(v);
    if (nearzero(denominator))
    {
        return Vector(); // You need to handle this error appropriately for your problem!
    }
    double t = u.dot(v) / denominator; // Create an extra variable to make things clearer
    return t*v;
}

Closest point on infinite line to point

While we often want the shortest distance betwen a point and a line segment, it is helpful to first derive the distance between a point and an infinite line, then adjust the equation.

Let our line be defined by two points, $\red{A_1}$ and $\orange{A_2}$. The line runs between these points and also extends infinitely in either direction. Given any point $\green{P}$, we want to find the closest point on our line (from which we can get its distance to $\green{P}$).

This looks very similar to our vector projection! We start by calculating $\yellow{\mathbf{u}}$ and $\purple{\mathbf{v}}$ as the vectors from $\red{A_1}$ to $\green{P}$ and $\orange{A_2}$ respectively:

$$\begin{align*} \yellow{\mathbf{u}} &= \green{P} - \red{A_1} \\ \purple{\mathbf{v}} &= \orange{A_2} - \red{A_1} \end{align*}$$

We can then calculate the coordinates of our closest point $\blue{P'}$ by adding the projected vector $\text{proj}_\mathbf{\purple{v}} \yellow{\mathbf{u}}$ back on to $\red{A_1}$:

$$\begin{align*} \blue{P'} &= \red{A_1} + \text{proj}_\mathbf{\purple{v}} \yellow{\mathbf{u}} \\ &= \red{A_1} + \frac{\yellow{\mathbf{u}} \cdot \purple{\mathbf{v}}}{\purple{\mathbf{v}} \cdot \purple{\mathbf{v}}} \purple{\mathbf{v}} \\ &= \red{A_1} + t \purple{\mathbf{v}} \\ &= \red{A_1} + t (\orange{A_2} - \red{A_1}) \end{align*}$$

Since $\purple{\mathbf{v}}$ was just the vector from $\red{A_1}$ to $\orange{A_2}$, our scaling factor $t$ is linearly interpolating between them. Values between $0$ and $1$ will be between the two original defining points, $t<0$ will be off one end, $t>1$ off the other.

Example Code

Vector CalcClosestPointToLine(Vector P, Vector A1, Vector A2)
{
    Vector u = P - A1;
    Vector v = A2 - A1;
    double denominator = v.dot(v);
    if (nearzero(denominator))
    {
        return A1; // You need to handle this error appropriately for your problem!
    }
    double t = u.dot(v) / denominator; // Create an extra variable to make things clearer
    return A1 + t*v;
    // Alternative return options that might make more sense for you:
    // return lerp(A1, A2, t);
    // return A1 + ProjectVectorOntoVector(u,v); // (Skipping the calculations)
}

tip

• If you already have your line defined as a point and a direction, you start with $\mathbf{v}$ already, no need to calculate it!
• If you also already know that $\mathbf{v}$ is a unit vector, you can use the knowledge that $(\mathbf{v} \cdot \mathbf{v}) = 1$ to simplify things. The denominator disappears completely and the result is $P' = (\mathbf{u} \cdot \mathbf{v}) \mathbf{v}$.

Closest point on line segment to point

What about a line that isn't infinite?

This is almost identical to the previous example, except we want to clamp the point between the endpoints of the segment - when the result would otherwise go outside the ends, the end becomes the closest point.

By seeing the previous result as an interpolation from $\red{A_1}$ to $\orange{A_2}$ by $t$, all we need to do to clamp our result point is to clamp $t$ between $0$ and $1$.

$$\begin{align*} \blue{P'} &=\red{A_1} + \text{clamp}_{0,1}(t) (\orange{A_2} - \red{A_1}) \\ &=\red{A_1} + \text{clamp}_{0,1}\left(\frac{\yellow{\mathbf{u}} \cdot \purple{\mathbf{v}}}{\purple{\mathbf{v}} \cdot \purple{\mathbf{v}}} \right) \purple{\mathbf{v}} \end{align*}$$

Where, again, we have:

$$\begin{align*} \yellow{\mathbf{u}} &= \green{P} - \red{A_1} \\ \purple{\mathbf{v}} &= \orange{A_2} - \red{A_1} \end{align*}$$

tip

In this case, if we divide by 0 (see warning on previous) we usually want to return the point that is the line segment, as it is guaranteed to be the closest.

Example Code

Vector ClosestPointToLineSegment(Vector P, Vector A1, Vector A2)
{
    Vector u = P - A1;
    Vector v = A2 - A1;
    double denominator = v.dot(v);
    if (nearzero(denominator))
    {
        return A1; // In this case returning A1 (or A2) is likely what you want
    }
    double t = u.dot(v) / denominator;
    return lerp(A1, A2, clamp01(t));
}

Projecting a point onto a plane

Before we move to the next problem with line segments, we need another tool - projecting a point onto a plane.

Let's say we have a plane defined by a point $\red{A}$ and a normal vector $\purple{\mathbf{n}}$, and we want to find $\orange{P'}$, the projection of a point $\green{P}$ onto the plane.

We start by creating a vector $\yellow{\mathbf{u}}$ from $\red{A}$ to $\green{P}$, and projecting it onto $\purple{\mathbf{n}}$. Since $\purple{\mathbf{n}}$ is a unit vector, this simplifies down:

$$\begin{align*} \yellow{\mathbf{u}} &= \green{P} - \red{A} \\ \blue{\mathbf{u}'} &= \text{proj}_{\purple{\mathbf{n}}} \yellow{\mathbf{u}} \\ &= (\yellow{\mathbf{u}} \cdot \purple{\mathbf{n}})\purple{\mathbf{n}} \end{align*}$$

Then, to get $\orange{P'}$ we just need to subtract $\blue{\mathbf{u}'}$ from $\green{P}$:

$$\orange{P'} = \green{P} - \blue{\mathbf{u}'}$$

tip

If you need the plane normal to a line segment defined by two points $A_1$ and $A_2$ (as we will in the next step), pick the one you want as $A$ (e.g. $A_1$) and then calculate the normal vector with:

$$\begin{align*} \mathbf{v} &= A_2-A_1 \\ \mathbf{n} &= \hat{\mathbf{v}} = \frac{\mathbf{v}}{|\mathbf{v}|} \end{align*}$$

Code example

Vector ProjectPointOntoPlane(Vector P, Vector A, Vector N)
{
    Vector u = P - A;
    Vector t = u.dot(N);
    return P - t*N;
}

Closest point on line segment to infinite line

Next we'll see how to find the closest point on a line segment to an infinite line. This probably has some practical use, but we are mostly using it as a stepping-stone to finding the closest points between two segments.

Let's say our infinite line is defined by two points $\green{L_1}$ and $\green{L_2}$, and our line segment by $\red{S_1}$ and $\red{S_2}$

This is a three-step process:

1. Project the two points of the line segment ($\red{S_1}$ and $\red{S_2}$) onto the plane defined by one point on the infinite line ($\green{L_1}$), and its normal ($\green{\hat{L_2 - L_1}}$) to create $\purple{S_1'}$ and $\purple{S_2'}$

2. Find $\orange{L'}$ the closest point between $\green{L_1}$ and the new projected line segment ($\purple{S_1'}$ and $\purple{S_2'}$).

3. If we use the same $t$ value to interpolate between the two original points ($\red{S_1}$ and $\red{S_2}$), we get $\blue{L''}$, the closest point on the original line segment.

tip

You might notice that we don't actually need $\orange{L'}$, just its $t$ value.

Example Code

Vector ClosestPointOnLineSegToLine(Vector L1, Vector L2, Vector S1, Vector S2)
{
    // Project the line segment onto the plane defined by the line
    Vector n = (L2 - L1).normalize();
    Vector S1a = ProjectPointOntoPlane(S1, L1, n);
    Vector S2a = ProjectPointOntoPlane(S2, L1, n);

    // Find the t value for the closest point in the projected space
    Vector u = L1 - S1a;
    Vector v = S2a - S1a;
    double denominator = v.dot(v);
    if (nearzero(denominator))
    {
        return S1; // You decide what to do here!
    }
    double t = u.dot(v) / denominator;

    // Use the t value to interpolate between the ORIGINAL points
    return lerp(S1, S2, clamp01(t));
}

Closest points between two line segments

Tip: Move the left-most green dot across to the right of the view to really see the steps below in action.

We're on the home stretch! This is a three-step process:

1. Find $\purple{B'}$, the closest point to the infinite line of one segment ($\red{A}$) on the finite segment of the other ($\green{B}$).
2. Find $\blue{A'}$, the closest point to $\purple{B'}$ on segment $\red{A}$. This is one of the the two final points.
3. Find $\orange{B''}$, the closest point to $\blue{A'}$ on segment $\green{B}$.

The points from steps 2 and 3 are your closest points!

tip

If at the end of step 1 you find that $0<t<1$, you can skip step 3, as $B''$ will be the same as $B'$!

If you know this will happen a lot it's worth taking into account in your code.

Code Example

(Vector,Vector) ClosestPointsBetweenLineSegments(Vector A1, Vector A2, Vector B1, Vector B2)
{
    // Closest point on B to infinite A
    Vector Bx = ClosestPointOnLineSegToLine(A1, A2, B1, B2);

    // Closest point on finite A to that
    Vector Ax = ClosestPointToLineSegment(Bx, A1, A2);

    // Closest point on finite B to that
    Vector Bxx = ClosestPointToLineSegment(Ax, B1, B2);

    return (Ax, Bxx);
}