查找两条 3D 线段之间的最短距离

Question

我有两条线段，由其起点/终点处的 3D 点表示。

线：

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

3D 点只是坐标 X、Y 和 Z 的 3 个双精度数。

3D点：

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

问题：

我可以找到两条“线”之间的距离以及该距离“线”的端点吗？[1

我拥有的：

目前，我可以使用此代码成功获取两条线之间的距离（改编自此处使用线段到线段部分）：

    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;
    }

我需要的：

有没有办法从上面的代码中确定位移矢量“dP”的端点？如果没有，任何人都可以建议一种更好的方法来查找最小距离和该距离的端点吗？

感谢您的阅读，并提前感谢您的任何建议！

非常感谢 @Isaac van Bakel 提供此解决方案背后的理论

这是我的完整代码：两条线之间的最短距离，由以最短距离连接它们的线表示。

课程：

1. Sharp3D.Math ：我将此参考用于 Vector3D，但实际上任何 3D 矢量类都可以工作。最重要的是，如果逐个元素进行减法，甚至不需要向量。

2. Point3D ：我的个人 Point3D 课程。您可以根据需要随意使用。

   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 ：我的个人 Line 课程。您可以根据需要随意使用。

   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));
           }
       }
   }
   

功能：

1. ClampPointToLine ：我编写的夹紧函数，用于将点夹紧到一条线上。

   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 ：返回表示两条线之间最短距离的线的函数。如果无法解决则返回 null。

   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 };
   }
   

执行：

Line shortestDistanceLine = distBetweenLines(l1, l2);

结果：

到目前为止，这在我的测试中是准确的。如果传递了两个相同的行，则返回 null。我很感激任何反馈！

Answer 1

两条斜线（不相交的线）之间的最短距离是垂直于两条斜线的线的距离。

如果我们有一条带有已知点 p1 和 p2 的线 l1，以及一条带有已知点 p3 和 p4 的线 l2：

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

因此，我们知道我们正在寻找的向量 v 垂直于这两个方向向量：

d1.v = 0 & d2.v = 0

或者，如果您愿意：

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

d2 也一样。

让我们取直线 l1、l2 上的点，其中 v 实际上垂直于该方向。我们将这两个点分别称为 i1 和 i2。

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.

由于 v 是 i1 和 i2 之间的向量（根据定义），我们得到 v = i2 - i1。

这给出了 v 的 x,y,z 向量的替换：

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

等等。

您现在可以将其代回点积方程中：

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

这将我们的方程数量减少到 2 个（两个点积方程），并且有两个未知数（m 和 n），因此您现在可以求解它们！

有了 m 和 n 后，您就可以通过返回 i1 和 i2 的原始计算来找到坐标。

如果您只想要 p1-p2 和 p3-p4 之间的线段上的点的最短距离，则可以将 i1 和 i2 夹在这些坐标范围之间，因为最短距离始终尽可能接近垂直线。