Logo_iSurf_org

Subscribe
to eAlerts

for Geom Site
Updates

CLICK HERE

 

Distance between 3D Lines & Segments

by Dan Sunday

 

 

We considered the Distance of a Point to a Line  and the Distance of a Point to a Segment in Algorithm 2. And we considered the Distance of a Point to a Plane in Algorithm 4. We now consider the distance between both infinite lines and finite line segments.

One sometimes has to compute the minimum distance separating two geometric objects; for example, in collision avoidance algorithms. These objects could be polygons (in 2D) or polyhedra (in 3D). The Euclidean distance between any two geometric objects is defined as the minimum distance between any two of their points. That is, for two geometric sets G1 and G2 (in any n-dimensional space), the distance between them is defined as:

Eqn_dGG

In this algorithm, we show how to efficiently compute this distance between lines, rays and segments, in any dimension. We do this by showing how to find two points, PC in G1 and QC in G2, where this minimum occurs; that is, where d(G1,G2) = d(PC, QC). It is not true that these points always exist for arbitrary geometric sets. But, they exist for many well-behaved geometric objects such as lines, planes, polygons, polyhedra, and "compact" (i.e.: closed & bounded) objects.

Additionally, we show how to compute the closest point of approach (CPA) between two points that are dynamically moving in straight lines. This is an important computation in collision detection, for example to determine how close two planes or two ships, represented as point "tracks", will get as they pass each other.

We use the parametric equation to represent lines, rays and segments, as described in the Algorithm 2 section on Lines, which also shows how to convert between this and other representations.


Distance between Lines

Consider two lines L1: P(s)=P0+s(P1-P0)=P0+su and L2: Q(t)=Q0+t(Q1-Q0)=Q0+tv. Let wst=Ps-Qt be a vector between points on the two lines. We want to find the w(s,t) that has a minimum length for all s and t. This can be computed using calculus [Eberly, 2001]. Here, we use a more geometric approach, and end up with the same result. A similar geometric approach was used by [Teller, 2000], but he used a cross product which restricts his method to 3D space whereas our method works in any dimension. Also, the solution given here and the Eberly result are faster than Teller's which computes intermediate planes and gets their intersections with the two lines.

In any n-dimensional space, the two lines L1 and L2 are closest at unique points PC = P(sC) and QC = Q(tC) for which w(sC, tC) is the unique minimum for w(s,t). Further, if L1 and L2 are not parallel and do not intersect each other, then the segment PCQC joining these points is uniquely simultaneously perpendicular to both lines. No other segment between L1 and L2 has this property. That is, the vector wC = w(sC, tC) is uniquely perpendicular to the line direction vectors u and v, and this is equivalent to it satisfying the two equations: u_dot_wC=0 and v_dot_wC=0.

Pic_d2lines

We can solve these two equations by substituting Eqn_lines0, where w0=P0-Q0, into each one to get two simultaneous linear equations:

Eqn_lines1

Then, putting a = u u, b = u v, c = v v, d = u w0, and e = v w0, we solve for sC and tC as:

Eqn_lines2

whenever the denominator acb2 is nonzero. Note that ac-b2=...=...ge.0 is always nonnegative. When acb2 = 0, the two equations are dependant, the two lines are parallel, and the distance between the lines is constant. We can solve for this parallel distance of separation by fixing the value of one parameter and using either equation to solve for the other. Selecting sC = 0, we get tC = d / b = e / c.

Having solved for sC and tC, we have the points PC and QC on the two lines L1 and L2 where they are closest to each other. Then the distance between them is given by:

Eqn_lines3


Distance between Segments and Rays

The distance between segments and rays may not be the same as the distance between their extended lines. The closest points on the extended infinite line may be outside the range of the segment or ray which is a restricted subset of the line. We represent the segment S1=[P0,P1] by P(s)=P0+s(P1-P0)=P0+su with s.ge-0.le-1. And the positive ray R1 (starting from P0) is given by the points P(s) with s.ge-0. Similarly, the segment S2=[Q0,Q1] is given by the points Q(t) with t.ge-0.le-1, and the ray R2 is given by points with t.ge-0.

The first step in computing a distance involving segments and/or rays is to get the closest points for the infinite lines that they lie on. So, we first compute sC and tC for L1 and L2, and if these are both in the range of the respective segment or ray, then they are also give closest points. But if they lie outside the range of either, then they are not and we have to determine new points that minimize wst=Ps-Qt over the ranges of interest. To do this, we first note that minimizing the length of w is the same as minimizingEqn_seg-quadratic which is a quadratic function of s and t. In fact, |w|2 defines a parabaloid over the (s,t)-plane with a minimum at C = (sC, tC), and which is strictly increasing along rays in the (s,t)-plane that start from C and go in any direction. But, when segments and/or rays are involved, we need the minimum over a subregion G of the (s,t)-plane, and the global absolute minimum at C may lie outside of G. However, in these cases, the minimum always occurs on the boundary of G, and in particular, on the part of G's boundary that is visible to C. That is, there is a line from C to the boundary point which is exterior to G, and we say that C can "see" points on this visible boundary of G.

To be more specific, suppose that we want the minimum distance between two finite segments S1 and S2. Then, we have that Eqn_G=unit_square is the unit square as shown in the diagram. The four edges of the square are given by s = 0, s = 1, t = 0, and t = 1. And, if C = (sC, tC) is outside G, then it can see at most two edges of G. If sC < 0, C can see the s = 0 edge; if sC > 1, C can see the s = 1 edge; and similarly for tC. Clearly, if C is not in G, then at least 1 and at most 2 of these inequalities are true, and they determine which edges of G are candidates for a minimum of |w|2.

Pic_d2segments

For each candidate edge, we use basic calculus to compute where the minimum occurs on that edge, either in its interior or at an endpoint. Consider the edge s = 0, along which Eqn_seg-quadratic-2. Taking the derivative with t we get a minimum when:

Eqn_segs1

which gives a minimum on the edge at (0, t0) where:

Eqn_segs2

If t0.ge-0.le-1, then this will be the minimum of |w|2 on G, and P(0) and Q(t0) are the two closest points of the two segments. However, if t0 is outside the edge, then an endpoint of the edge, (0,0) or (0,1), is the minimum along that edge; and further, we will have to check a second visible edge in case the true absolute minimum is on it. The other edges are treated in a similar manner.

Putting it all together by testing all candidate edges, we end up with a relatively simple algorithm that only has a few cases to check. An analogous approach is given by [Eberly, 2001], but it has more cases which makes it more complicated to implement.

Other distance algorithms, such as line-to-ray or ray-to-segment, are similar in spirit, but have fewer boundary tests to make than the segment-to-segment distance algorithm. See dist3D_Segment_to_Segment() for our implementation.


Closest Point of Approach (CPA)

The "Closest Point of Approach" refers to the positions at which two dynamically moving objects reach their closest possible distance. This is an important calculation for collision avoidance. In many cases of interest, the objects, referred to as "tracks", are points moving in two fixed directions at fixed speeds. That means that the two points are moving along two lines in space. However, their closest distance is not the same as the closest distance between the lines since the distance between the points must be computed at the same moment in time. So, even in 2D with two lines that intersect, points moving along these lines may remain far apart. But if one of the tracks is stationary, then the CPA of another moving track is at the base of the perpendicular from the first track to the second's line of motion.

Consider two dynamically changing points whose positions at time t are P(t) and Q(t). Let their positions at time t = 0 be P0 and Q0; and let their velocity vectors per unit of time be u and v. Then, the equations of motion for these two points are P(t)=P0+tu and Q(t)=Q0+tv, which are the familiar parametric equations for the lines. However, the two equations are coupled by having a common parameter t. So, at time t, the distance between them is d(t) = |P(t) – Q(t)| = |w(t)| where w(t)=w0+t(u-v) with w0=P0-Q0.

Pic_cpa

Now, since d(t) is a minimum when D(t) = d(t)2 is a minimum, we can compute:

Eqn_cpa1

which has a minimum when

Eqn_cpa2

which can be solved to get the time of CPA to be:

Eqn_cpa3

whenever |u v| is nonzero. If |u v| = 0, then the two point tracks are traveling in the same direction at the same speed, and will always remain the same distance apart, so one can use tCPA = 0. In both cases we have that:

Eqn_cpa4

Note that when tCPA < 0, then the CPA has already occurred in the past, and the two tracks are getting further apart as they move on in time.


Implementations

Here are some sample "C++" implementations of these algorithms.

// Copyright 2001 softSurfer, 2012 Dan Sunday
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.
 

// Assume that classes are already given for the objects:
//    Point and Vector with
//        coordinates {float x, y, z;}
//        operators for:
//            Point   = Point Vector
//            Vector =  Point - Point
//            Vector =  Vector Vector
//            Vector =  Scalar * Vector
//    Line and Segment with defining points {Point  P0, P1;}
//    Track with initial position and velocity vector
//            {Point P0;  Vector v;}
//===================================================================
 

#define SMALL_NUM   0.00000001 // anything that avoids division overflow
// dot product (3D) which allows vector operations in arguments
#define dot(u,v)   ((u).x * (v).x + (u).y * (v).y + (u).z * (v).z)
#define norm(v)    sqrt(dot(v,v))  // norm = length of  vector
#define d(u,v)     norm(u-v)        // distance = norm of difference
#define abs(x)     ((x) >= 0 ? (x) : -(x))   //  absolute value
 


// dist3D_Line_to_Line(): get the 3D minimum distance between 2 lines
//    Input:  two 3D lines L1 and L2
//    Return: the shortest distance between L1 and L2
float
dist3D_Line_to_Line( Line L1, Line L2)
{
    Vector   u = L1.P1 - L1.P0;
    Vector   v = L2.P1 - L2.P0;
    Vector   w = L1.P0 - L2.P0;
    float    a = dot(u,u);         // always >= 0
    float    b = dot(u,v);
    float    c = dot(v,v);         // always >= 0
    float    d = dot(u,w);
    float    e = dot(v,w);
    float    D = a*c - b*b;        // always >= 0
    float    sc, tc;

    // compute the line parameters of the two closest points
    if (D < SMALL_NUM) {          // the lines are almost parallel
        sc = 0.0;
        tc = (b>c ? d/b : e/c);    // use the largest denominator
    }
    else {
        sc = (b*e - c*d) / D;
        tc = (a*e - b*d) / D;
    }

    // get the difference of the two closest points
    Vector   dP = w + (sc * u) - (tc * v);  // =  L1(sc) - L2(tc)

    return norm(dP);   // return the closest distance
}
//===================================================================


// dist3D_Segment_to_Segment(): get the 3D minimum distance between 2 segments
//    Input:  two 3D line segments S1 and S2
//    Return: the shortest distance between S1 and S2
float
dist3D_Segment_to_Segment( Segment S1, Segment S2)
{
    Vector   u = S1.P1 - S1.P0;
    Vector   v = S2.P1 - S2.P0;
    Vector   w = S1.P0 - S2.P0;
    float    a = dot(u,u);         // always >= 0
    float    b = dot(u,v);
    float    c = dot(v,v);         // always >= 0
    float    d = dot(u,w);
    float    e = dot(v,w);
    float    D = a*c - b*b;        // always >= 0
    float    sc, sN, sD = D;       // sc = sN / sD, default sD = D >= 0
    float    tc, tN, tD = D;       // tc = tN / tD, default tD = D >= 0

    // compute the line parameters of the two closest points
    if (D < SMALL_NUM) { // the lines are almost parallel
        sN = 0.0;         // force using point P0 on segment S1
        sD = 1.0;         // to prevent possible division by 0.0 later
        tN = e;
        tD = c;
    }
    else {                 // get the closest points on the infinite lines
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {        // sc < 0 => the s=0 edge is visible
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {  // sc > 1  => the s=1 edge is visible
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0) {            // tc < 0 => the t=0 edge is visible
        tN = 0.0;
        // recompute sc for this edge
        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {      // tc > 1  => the t=1 edge is visible
        tN = tD;
        // recompute sc for this edge
        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d +  b);
            sD = a;
        }
    }
    // finally do the division to get sc and tc
    sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
    tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);

    // get the difference of the two closest points
    Vector   dP = w + (sc * u) - (tc * v);  // =  S1(sc) - S2(tc)

    return norm(dP);   // return the closest distance
}
//===================================================================


// cpa_time(): compute the time of CPA for two tracks
//    Input:  two tracks Tr1 and Tr2
//    Return: the time at which the two tracks are closest
float
cpa_time( Track Tr1, Track Tr2 )
{
    Vector   dv = Tr1.v - Tr2.v;

    float    dv2 = dot(dv,dv);
    if (dv2 < SMALL_NUM)      // the  tracks are almost parallel
        return 0.0;             // any time is ok.  Use time 0.

    Vector   w0 = Tr1.P0 - Tr2.P0;
    float    cpatime = -dot(w0,dv) / dv2;

    return cpatime;             // time of CPA
}
//===================================================================


// cpa_distance(): compute the distance at CPA for two tracks
//    Input:  two tracks Tr1 and Tr2
//    Return: the distance for which the two tracks are closest
float
cpa_distance( Track Tr1, Track Tr2 )
{
    float    ctime = cpa_time( Tr1, Tr2);
    Point    P1 = Tr1.P0 + (ctime * Tr1.v);
    Point    P2 = Tr2.P0 + (ctime * Tr2.v);

    return d(P1,P2);            // distance at CPA
}
//===================================================================


References

David Eberly, "Distance Methods" in 3D Game Engine Design (2006)

Seth Teller, line_line_closest_points3d() (2000) cited in the Graphics  Algorithms FAQ (2001)

 

 

© Copyright 2012 Dan Sunday, 2001 softSurfer