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:
- 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!