Finding The Shortest Distance Between Two 3D Line

2019-04-13 09:20发布

问题:

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'. [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:

  1. 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.
  2. 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 };
        }
    
    }
    
  3. 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:

  1. 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;
    }
    
  2. 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!

回答1:

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.