Mar
10
2009

Code: Shortest distance between any two line segments

I recently needed to find the shortest distance between any two line segments in 3D space.

I managed to find some C++ code online at http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm which I converted to C#.

It’s fairly easy to use. If you only need it for 2D space, just set the Z values to 0.

Please post a comment if you find this useful.

Here is the code:

/*
* Converted to C# by John Burns from C++ sourced from:
* http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
*
* Original C++ Copyright Notice
* ===================
* Copyright 2001, softSurfer (www.softsurfer.com)
* 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.
*/
 
class point
{
    public double x;
    public double y;
    public double z;
}
 
class line
{
    public double x1;
    public double y1;
    public double z1;
    public double x2;
    public double y2;
    public double z2;
}
 
double getShortestDistance(line line1, line line2)
{
    double EPS = 0.00000001;
 
    point delta21 = new point();
    delta21.x = line1.x2 - line1.x1;
    delta21.y = line1.y2 - line1.y1;
    delta21.x = line1.z2 - line1.z1;
 
    point delta41 = new point();
    delta41.x = line2.x2 - line2.x1;
    delta41.y = line2.y2 - line2.y1;
    delta41.z = line2.z2 - line2.z1;
 
    point delta13 = new point();
    delta13.x = line1.x1 - line2.x1;
    delta13.y = line1.y1 - line2.y1;
    delta13.z = line1.z1 - line2.z1;
 
    double a = dot(delta21, delta21);
    double b = dot(delta21, delta41);
    double c = dot(delta41, delta41);
    double d = dot(delta21, delta13);
    double e = dot(delta41, delta13);
    double D = a * c - b * b;
 
    double sc, sN, sD = D;
    double tc, tN, tD = D;
 
    if (D < EPS)
    {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else
    {
        sN = (b * e - c * d);
        tN = (a * e - b * d);
        if (sN < 0.0)
        {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD)
        {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }
 
    if (tN < 0.0)
    {
        tN = 0.0;
 
        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else
        {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD)
    {
        tN = tD;
        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else
        {
            sN = (-d + b);
            sD = a;
        }
    }
 
    if (Math.Abs(sN) < EPS) sc = 0.0;
    else sc = sN / sD;
    if (Math.Abs(tN) < EPS) tc = 0.0;
    else tc = tN / tD;
 
    point dP = new point();
    dP.x = delta13.x + (sc * delta21.x) - (tc * delta41.x);
    dP.y = delta13.y + (sc * delta21.y) - (tc * delta41.y);
    dP.z = delta13.z + (sc * delta21.z) - (tc * delta41.z);
 
    return Math.Sqrt(dot(dP, dP));
}
 
private double dot(point c1, point c2)
{
    return (c1.x * c2.x + c1.y * c2.y + c1.z * c2.z);
}
 
private double norm(point c1)
{
    return Math.Sqrt(dot(c1, c1));
}
Written by John in: C# Tips |

5 Comments »

  • pablo

    Very useful, thank a lot!

    Comment | 14 November 2012
  • Bob

    In shortest distance between any two line segments in 3D space.

    I think

    delta21.x = line1.z2 – line1.z1;

    should be

    delta21.z = line1.z2 – line1.z1;

    Comment | 7 December 2012
  • Sergi

    Very useful, thank a lot John !

    Comment | 6 February 2013
  • Belal

    Thanks man, very very Very useful 🙂

    Comment | 10 October 2013
  • Dek

    Thanks!

    Comment | 22 March 2014

RSS feed for comments on this post. TrackBack URL

Leave a comment