Finding The Shortest Distance Between Two 3D Line Segments

Question

I have two Line Segments, represented by a 3D point at their beginning/end points.

Line:

class Line
{
    public string Name { get; set; }
    public Point3D Start { get; set; } = new Point3D();
    public Point3D End { get; set; } = new Point3D();
}

The 3D points are just 3 doubles for coordinates X,Y and Z.

3DPoint:

class Point3D
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }
}

The Question:

Can I find the distance between two 'Lines' and the endpoints of that distance 'Line'. [ Here is an Image to Better Illustrate What I am trying to Achieve 1

What I have:

Currently, I can successfully get the distance between the two lines with this code (Adapted From Here Using the Segment To Segment Section):

    public double lineNearLine(Line l1, Line l2)
    {
        Vector3D uS = new Vector3D { X = l1.Start.X, Y = l1.Start.Y, Z = l1.Start.Z };
        Vector3D uE = new Vector3D { X = l1.End.X, Y = l1.End.Y, Z = l1.End.Z };
        Vector3D vS = new Vector3D { X = l2.Start.X, Y = l2.Start.Y, Z = l2.Start.Z };
        Vector3D vE = new Vector3D { X = l2.End.X, Y = l2.End.Y, Z = l2.End.Z };
        Vector3D w1 = new Vector3D { X = l1.Start.X, Y = l1.Start.Y, Z = l1.Start.Z };
        Vector3D w2 = new Vector3D { X = l2.Start.X, Y = l2.Start.Y, Z = l2.Start.Z };
        Vector3D u = uE - uS;
        Vector3D v = vE - vS;
        Vector3D w = w1 - w2;
        double a = Vector3D.DotProduct(u, u);
        double b = Vector3D.DotProduct(u, v);
        double c = Vector3D.DotProduct(v, v);
        double d = Vector3D.DotProduct(u, w);
        double e = Vector3D.DotProduct(v, w);
        double D = a * c - b * b;
        double sc, sN, sD = D;
        double tc, tN, tD = D;
        if (D < 0.01)
        {
            sN = 0;
            sD = 1;
            tN = e;
            tD = c;
        }
        else
        {
            sN = (b * e - c * d);
            tN = (a * e - b * d);
            if (sN < 0)
            {
                sN = 0;
                tN = e;
                tD = c;
            }
            else if (sN > sD)
            {
                sN = sD;
                tN = e + b;
                tD = c;
            }
        }
        if (tN < 0)
        {
            tN = 0;
            if (-d < 0)
            {
                sN = 0;
            }
            else if (-d > a)
            {
                sN = sD;
            }
            else
            {
                sN = -d;
                sD = a;
            }
        }
        else if (tN > tD)
        {
            tN = tD;
            if ((-d + b) < 0)
            {
                sN = 0;
            }
            else if ((-d + b) > a)
            {
                sN = sD;
            }
            else
            {
                sN = (-d + b);
                sD = a;
            }
        }
        if (Math.Abs(sN) < 0.01)
        {
            sc = 0;
        }
        else
        {
            sc = sN / sD;
        }
        if (Math.Abs(tN) < 0.01)
        {
            tc = 0;
        }
        else
        {
            tc = tN / tD;
        }
        Vector3D dP = w + (sc * u) - (tc * v);
        double distance1 = Math.Sqrt(Vector3D.DotProduct(dP, dP));
        return distance1;
    }

What I Need:

Is there any way to determine the endpoints of the displacement vector 'dP' from the code above? If not, can anyone suggest a better method for finding minimum distance and the endpoints of that distance?

Thank you for Reading, and Thanks in advance for any suggestions!

An Enormous Thank You to @Isaac van Bakel for the theory behind this Solution

Here is my code complete: Shortest distance between two lines represented by the line that connects them at that shortest distance.

Classes:

Sharp3D.Math : I use this reference for Vector3D, but really any 3D vector class will work. On top of that, the vectors aren't even required if you do the subtraction element by element.

Point3D : My Personal Point3D class. Feel free to use as much or little as you want.

class Point3D
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }
    public  Vector3D getVector()
    {
        return new Vector3D { X = this.X, Y = this.Y, Z = this.Z };
    }

}

Line : My Personal Line class. Feel free to use as much or little as you want.

class Line
{
    public string Name { get; set; }
    public Point3D Start { get; set; } = new Point3D();
    public Point3D End { get; set; } = new Point3D();
    public double Length
    {
        get
        {
            return Math.Sqrt(Math.Pow((End.X - Start.X), 2) + Math.Pow((End.Y - Start.Y), 2));
        }
    }
}

Functions:

ClampPointToLine : Clamping function I wrote to clamp a point to a line.

public Point3D ClampPointToLine(Point3D pointToClamp, Line lineToClampTo)
{
    Point3D clampedPoint = new Point3D();
    double minX, minY, minZ, maxX, maxY, maxZ;
    if(lineToClampTo.Start.X <= lineToClampTo.End.X)
    {
        minX = lineToClampTo.Start.X;
        maxX = lineToClampTo.End.X;
    }
    else
    {
        minX = lineToClampTo.End.X;
        maxX = lineToClampTo.Start.X;
    }
    if (lineToClampTo.Start.Y <= lineToClampTo.End.Y)
    {
        minY = lineToClampTo.Start.Y;
        maxY = lineToClampTo.End.Y;
    }
    else
    {
        minY = lineToClampTo.End.Y;
        maxY = lineToClampTo.Start.Y;
    }
    if (lineToClampTo.Start.Z <= lineToClampTo.End.Z)
    {
        minZ = lineToClampTo.Start.Z;
        maxZ = lineToClampTo.End.Z;
    }
    else
    {
        minZ = lineToClampTo.End.Z;
        maxZ = lineToClampTo.Start.Z;
    }
    clampedPoint.X = (pointToClamp.X < minX) ? minX : (pointToClamp.X > maxX) ? maxX : pointToClamp.X;
    clampedPoint.Y = (pointToClamp.Y < minY) ? minY : (pointToClamp.Y > maxY) ? maxY : pointToClamp.Y;
    clampedPoint.Z = (pointToClamp.Z < minZ) ? minZ : (pointToClamp.Z > maxZ) ? maxZ : pointToClamp.Z;
    return clampedPoint;
}

distanceBetweenLines : The function that returns the line that represents the shortest distance between two lines. Returns null if unsolvable.

public Line distBetweenLines(Line l1, Line l2)
{
    Vector3D p1, p2, p3, p4, d1, d2;
    p1 = l1.Start.getVector();
    p2 = l1.End.getVector();
    p3 = l2.Start.getVector();
    p4 = l2.End.getVector();
    d1 = p2 - p1;
    d2 = p4 - p3;
    double eq1nCoeff = (d1.X * d2.X) + (d1.Y * d2.Y) + (d1.Z * d2.Z);
    double eq1mCoeff = (-(Math.Pow(d1.X, 2)) - (Math.Pow(d1.Y, 2)) - (Math.Pow(d1.Z, 2)));
    double eq1Const = ((d1.X * p3.X) - (d1.X * p1.X) + (d1.Y * p3.Y) - (d1.Y * p1.Y) + (d1.Z * p3.Z) - (d1.Z * p1.Z));
    double eq2nCoeff = ((Math.Pow(d2.X, 2)) + (Math.Pow(d2.Y, 2)) + (Math.Pow(d2.Z, 2)));
    double eq2mCoeff = -(d1.X * d2.X) - (d1.Y * d2.Y) - (d1.Z * d2.Z);
    double eq2Const = ((d2.X * p3.X) - (d2.X * p1.X) + (d2.Y * p3.Y) - (d2.Y * p2.Y) + (d2.Z * p3.Z) - (d2.Z * p1.Z));
    double[,] M = new double[,] { { eq1nCoeff, eq1mCoeff, -eq1Const }, { eq2nCoeff, eq2mCoeff, -eq2Const } };
    int rowCount = M.GetUpperBound(0) + 1;
    // pivoting
    for (int col = 0; col + 1 < rowCount; col++) if (M[col, col] == 0)
        // check for zero coefficients
        {
            // find non-zero coefficient
            int swapRow = col + 1;
            for (; swapRow < rowCount; swapRow++) if (M[swapRow, col] != 0) break;

            if (M[swapRow, col] != 0) // found a non-zero coefficient?
            {
                // yes, then swap it with the above
                double[] tmp = new double[rowCount + 1];
                for (int i = 0; i < rowCount + 1; i++)
                { tmp[i] = M[swapRow, i]; M[swapRow, i] = M[col, i]; M[col, i] = tmp[i]; }
            }
            else return null; // no, then the matrix has no unique solution
        }

    // elimination
    for (int sourceRow = 0; sourceRow + 1 < rowCount; sourceRow++)
    {
        for (int destRow = sourceRow + 1; destRow < rowCount; destRow++)
        {
            double df = M[sourceRow, sourceRow];
            double sf = M[destRow, sourceRow];
            for (int i = 0; i < rowCount + 1; i++)
                M[destRow, i] = M[destRow, i] * df - M[sourceRow, i] * sf;
        }
    }

    // back-insertion
    for (int row = rowCount - 1; row >= 0; row--)
    {
        double f = M[row, row];
        if (f == 0) return null;

        for (int i = 0; i < rowCount + 1; i++) M[row, i] /= f;
        for (int destRow = 0; destRow < row; destRow++)
        { M[destRow, rowCount] -= M[destRow, row] * M[row, rowCount]; M[destRow, row] = 0; }
    }
    double n = M[0, 2];
    double m = M[1, 2];
    Point3D i1 = new Point3D { X = p1.X + (m * d1.X), Y = p1.Y + (m * d1.Y), Z = p1.Z + (m * d1.Z) };
    Point3D i2 = new Point3D { X = p3.X + (n * d2.X), Y = p3.Y + (n * d2.Y), Z = p3.Z + (n * d2.Z) };
    Point3D i1Clamped = ClampPointToLine(i1, l1);
    Point3D i2Clamped = ClampPointToLine(i2, l2);
    return new Line { Start = i1Clamped, End = i2Clamped };
}

Implementation:

Line shortestDistanceLine = distBetweenLines(l1, l2);

Results:

So far this has been accurate in my testing. Returns null if passed two identical lines. I appreciate any feedback!

in that image it looks like the desired points are the center of the lines... is that just in this example? because if its random lines it will almost never be the case. that is the shortest possible distance between the two lines — Neil, Jul 28 '16 at 14:13
Yes, it was made in paint to be an arbitrary example of what I'm trying to obtain. The lines are not generated randomly, but are very arbitrary in their positioning. — Kikootwo, Jul 28 '16 at 14:20
Just to clarify, are you saying it is always the midpoints of the line segments that you are seeking? — Chris Dunaway, Jul 28 '16 at 14:48
No. I am seeking the shortest distance between the red and green line. I drew an arbitrary picture to illustrate what I was looking for. If the shortest distance HAPPENS to be the midpoint, then yes. Otherwise, all i'm interested in is the shortest distance between those two lines. — Kikootwo, Jul 28 '16 at 14:49
You say you have two __line segments__, not two (unbounded) lines. Can you clarify if you need the distance between the line segments, or the distance between the lines generated by the line segments? Example: If one segment goes from `(0,0,0)` to `(1,0,0)` and another goes from `(0,100,1)` to `(0,101,1)` (both segments are horizontal), what should the distance be? It would be `1` if you took distance between the full lines; and it would be just over `100` if restricting to the segments. — Jeppe Stig Nielsen, Jul 28 '16 at 15:16
Yes, the distance between the two line segments. Not anything outside the beginning and ending points of that segment. The segments are in 3D space are the way they are generated they should never be parallel. But, for a more applicable solution, I would say the distance would be 99 units. From end point to end point. — Kikootwo, Jul 28 '16 at 15:20
Strongly related: [Calculating the shortest distance between two lines (line segments) in 3D](http://stackoverflow.com/questions/627563/) — Jeppe Stig Nielsen, Jul 28 '16 at 15:36
I agree, but my post is an extension of that. I already have the distance, I need the endpoints of that distance. I even used the same reference as that post. — Kikootwo, Jul 28 '16 at 16:46
just for information : Line : " double eq2Const = ((d2.X * p3.X) - (d2.X * p1.X) + (d2.Y * p3.Y) - (d2.Y * p2.Y) + (d2.Z * p3.Z) - (d2.Z * p1.Z));" should be : double eq2Const = ((d2.X * p3.X) - (d2.X * p1.X) + (d2.Y * p3.Y) - (d2.Y * p1.Y) + (d2.Z * p3.Z) - (d2.Z * p1.Z)); you exchanged a p2 and a p1 — L Chougrani, Feb 14 '23 at 06:52

score 4 · Accepted Answer · answered Jul 28 '16 at 15:08

4

The shortest distance between two skew lines (lines which don't intersect) is the distance of the line which is perpendicular to both of them.

If we have a line l1 with known points p1 and p2, and a line l2 with known points p3 and p4:

The direction vector of l1 is p2-p1, or d1.
The direction vector of l2 is p4-p3, or d2.

We therefore know that the vector we are looking for, v, is perpendicular to both of these direction vectors:

d1.v = 0 & d2.v = 0

Or, if you prefer:

d1x*vx + d1y*vy + d1z*vz = 0

And the same for d2.

Let's take the point on the lines l1, l2 where v is actually perpendicular to the direction. We'll call these two points i1 and i2 respectively.

Since i1 lies on l1, we can say that i1 = p1 + m*d1, where m is some number.
Similarly, i2 = p3 + n*d2, where n is another number.

Since v is the vector between i1 and i2 (by definition) we get that v = i2 - i1.

This gives the substitutions for the x,y,z vectors of v:

vx = i2x - i1x = (p3x + n*d2x) - (p1x + m*d1x)

and so on.

Which you can now substitute back into your dot product equation:

d1x * ( (p3x + n*d2x) - (p1x + m*d1x) ) + ... = 0

This has reduced our number of equations to 2 (the two dot product equations) with two unknowns (m and n), so you can now solve them!

Once you have m and n, you can find the coordinates by going back to the original calculation of i1 and i2.

If you only wanted the shortest distance for points on the segment between p1-p2 and p3-p4, you can clamp i1 and i2 between these ranges of coordinates, since the shortest distance will always be as close to the perpendicular as possible.

answered Jul 28 '16 at 15:08

Isaac van Bakel

1,772
10
22

This is great, I just had a quick clarification. Even though they are skew line segments, will the shortest distance between them still always be perpendicluar to both lines? – Kikootwo Jul 28 '16 at 15:17
1

This depends on what I talked about in the last part of the answer - do you want the points to be limited to be in the ranges p1 to p2 and p3 to p4 for each end of the line? If yes, it won't always be perpendicular, but it will be as close as possible. If no, it will always be perpendicular. – Isaac van Bakel Jul 28 '16 at 15:18
Okay, so yes, I want it to always be limited to the ranged of p1-p2 and p3-4. You mention 'clamp' but I'm not positive what that is. – Kikootwo Jul 28 '16 at 15:26
Clamping is [restricting a value to being between two other values](https://msdn.microsoft.com/en-us/library/microsoft.xna.framework.mathhelper.clamp.aspx) which you could do by checking the distance. If the distance between i1 and p1 and the distance between i1 and p2 are both smaller than the distance from p1 to p2, i1 is in the segment. Otherwise, you'd check the one which is too big, and restrict based on that i.e. if the distance from p2 is too big, the point must be further on the line that p1, and you can clamp to p1. – Isaac van Bakel Jul 28 '16 at 15:30
Awesome! thanks so much for your help. I'm gonna go ahead and accept this answer, and then go try out this solution. I'll report back my findings and update my post to code-complete for others seeking information on the topic. – Kikootwo Jul 28 '16 at 15:33
Added Code Complete Solution to Question. Thanks again for your help! – Kikootwo Jul 29 '16 at 14:22

score 1 · Answer 2 · answered Sep 16 '21 at 06:38

The code above return a wrong value so I take the idea from Python code (Shortest distance between two line segments) and I converted for C#. It necessary to use Numpy lib for C#:

public static Tuple<NDarray, NDarray, NDarray> closestDistanceBetweenLines(
        NDarray a0,
        NDarray a1,
        NDarray b0,
        NDarray b1,
        bool clampAll = false,
        bool clampA0 = false,
        bool clampA1 = false,
        bool clampB0 = false,
        bool clampB1 = false)
{
    // If clampAll=True, set all clamps to True
    if (clampAll)
    {
        clampA0 = true;
        clampA1 = true;
        clampB0 = true;
        clampB1 = true;
    }

    // Calculate denomitator
    NDarray A = a1 - a0;
    NDarray B = b1 - b0;
    NDarray magA = np.linalg.norm(A);
    NDarray magB = np.linalg.norm(B);
    NDarray _A = A / magA;
    NDarray _B = B / magB;
    NDarray cross = np.cross(_A, _B);
    double denom = Math.Pow((float)np.linalg.norm(cross), 2);

    // If lines are parallel (denom=0) test if lines overlap.
    // If they don't overlap then there is a closest point solution.
    // If they do overlap, there are infinite closest positions, but there is a closest distance
    if (denom == 0)
    {
        NDarray d0 = np.dot(_A, b0 - a0);

        // Overlap only possible with clamping
        if (clampA0 || clampA1 || clampB0 || clampB1)
        {
            NDarray d1 = np.dot(_A, b1 - a0);
            // Is segment B before A?              
            if ((float)d0 <= 0F || (float)d0 >= (float)d1)
            {
                if (clampA0 && clampB1)
                {
                    if ( (float)np.absolute(d0) < (float)np.absolute(d1) ) 
                    {
                        return Tuple.Create(a0, b0, np.linalg.norm(a0 - b0));
                    }
                    return Tuple.Create(a0, b1, np.linalg.norm(a0 - b1));
                }
            }
            else if ((float)d0 >= (float)magA || (float)d0 <= (float)d1)
            {
                // Is segment B after A?
                if (clampA1 && clampB0)
                {
                    if ((float)np.absolute(d0) < (float)np.absolute(d1))
                    {
                        return Tuple.Create(a1, b0, np.linalg.norm(a1 - b0));
                    }
                    return Tuple.Create(a1, b1, np.linalg.norm(a1 - b1));
                }
            }
        }

        // Segments overlap, return distance between parallel segments;
        NDarray vuoto1 = np.array(new[] { 0 });
        return Tuple.Create(vuoto1, vuoto1, np.linalg.norm(d0 * _A + a0 - b0));
    }

    // Lines criss-cross: Calculate the projected closest points
    NDarray t = b0 - a0;
    
    var ArrFordetA = np.array(new float[,] {    { (float)t[0], (float)t[1], (float)t[2] },
                                                { (float)_B[0], (float)_B[1], (float)_B[2] },
                                                { (float)cross[0], (float)cross[1], (float)cross[2] }   });
    
    NDarray detA = np.linalg.det(ArrFordetA);
    

    var ArrFordetB = np.array(new float[,] {    { (float)t[0], (float)t[1], (float)t[2] },
                                                { (float)_A[0], (float)_A[1], (float)_A[2] },
                                                { (float)cross[0], (float)cross[1], (float)cross[2] }   });

    NDarray detB = np.linalg.det(ArrFordetB);
    
    var t0 = detA / denom;
    var t1 = detB / denom;
    var pA = a0 + _A * t0;  // Projected closest point on segment A
    var pB = b0 + _B * t1;  // Projected closest point on segment B

    // Clamp projections
    if (clampA0 || clampA1 || clampB0 || clampB1)
    {
        if (clampA0 && (float)t0 < 0)
        {
            pA = a0;
        }
        else if (clampA1 && (float)t0 > (float)magA)
        {
            pA = a1;
        }
        if (clampB0 && (float)t1 < 0)
        {
            pB = b0;
        }
        else if (clampB1 && (float)t1 > (float)magB)
        {
            pB = b1;
        }
        // Clamp projection A
        if (clampA0 && (float)t0 < 0 || clampA1 && (float)t0 > (float)magA)
        {
            NDarray dot = np.dot(_B, pA - b0);
            if ( clampB0 && (float)dot < 0)
            {
                dot = (NDarray)0;
            }
            else if (clampB1 && (float)dot > (float)magB)
            {
                dot = magB;
            }
            pB = b0 + _B * dot;
        }
        // Clamp projection B
        if ( clampB0  && (float)t1 < 0 || clampB1 && (float)t1 > (float)magB)
        {
            NDarray dot = np.dot(_A, pB - a0);
            if (clampA0  && (float)dot < 0)
            {
                dot = (NDarray)0;
            }
            else if (clampA1 && (float)dot > (float)magA)
            {
                dot = magA;
            }
            pA = a0 + _A * dot;
        }
    }

    return Tuple.Create(pA, pB, np.linalg.norm(pA - pB));
}

to call this function use:

private void button1_Click(object sender, EventArgs e)
{
    NDarray a1 = np.array(new[] { 13.43, 21.77, 46.81 });
    NDarray a0 = np.array(new[] { 27.83, 31.74, -26.60 });
    NDarray b0 = np.array(new[] { 77.54, 7.53, 6.22 });
    NDarray b1 = np.array(new[] { 26.99, 12.39, 11.18 });
    
    Debug.WriteLine("-----------------  True: -----------------");
    Debug.WriteLine(closestDistanceBetweenLines(a0, a1, b0, b1, true));
    Debug.WriteLine("---------------------------------------------");
    Debug.WriteLine("");

    Debug.WriteLine("-----------------  False: -----------------");
    Debug.WriteLine(closestDistanceBetweenLines(a0, a1, b0, b1, false));
    Debug.WriteLine("---------------------------------------------");

    Tuple<NDarray, NDarray, NDarray> RisultatoTrue = closestDistanceBetweenLines(a0, a1, b0, b1, true);     
    
    var Pa = np.array(RisultatoTrue.Item1);
    Debug.WriteLine("Start point X:" + Pa[0].ToString());
    Debug.WriteLine("Start point Y:" + Pa[1].ToString());
    Debug.WriteLine("Start point Z:" + Pa[2].ToString());

    var Pb = np.array(RisultatoTrue.Item2);
    Debug.WriteLine("End point X:" + Pb[0].ToString());
    Debug.WriteLine("End point Y:" + Pb[1].ToString());
    Debug.WriteLine("End point Z:" + Pb[2].ToString());

    var dist = np.array(RisultatoTrue.Item3);
    Debug.WriteLine("Distance:" + dist.ToString());
}

Finding The Shortest Distance Between Two 3D Line Segments

2 Answers2

Linked