public static double DistancePointSegment(Coordinate p, Coordinate A, Coordinate B) { // if start = end, then just compute distance to one of the endpoints if (A.Equals3D(B)) return Distance(p, A); // otherwise use comp.graphics.algorithms Frequently Asked Questions method /* * (1) r = AC dot AB * --------- * ||AB||^2 * * r has the following meaning: * r=0 P = A * r=1 P = B * r<0 P is on the backward extension of AB * r>1 P is on the forward extension of AB * 0<r<1 P is interior to AB */ var len2 = (B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y) + (B.Z - A.Z) * (B.Z - A.Z); if (Double.IsNaN(len2)) throw new ArgumentException("Ordinates must not be NaN"); var r = ((p.X - A.X) * (B.X - A.X) + (p.Y - A.Y) * (B.Y - A.Y) + (p.Z - A.Z) * (B.Z - A.Z)) / len2; if (r <= 0.0) return Distance(p, A); if (r >= 1.0) return Distance(p, B); // compute closest point q on line segment var qx = A.X + r * (B.X - A.X); var qy = A.Y + r * (B.Y - A.Y); var qz = A.Z + r * (B.Z - A.Z); // result is distance from p to q var dx = p.X - qx; var dy = p.Y - qy; var dz = p.Z - qz; return Math.Sqrt(dx * dx + dy * dy + dz * dz); }
/** * Computes the distance between two 3D segments. * * @param A the start point of the first segment * @param B the end point of the first segment * @param C the start point of the second segment * @param D the end point of the second segment * @return the distance between the segments */ public static double DistanceSegmentSegment( Coordinate A, Coordinate B, Coordinate C, Coordinate D) { /** * This calculation is susceptible to roundoff errors when * passed large ordinate values. * It may be possible to improve this by using {@link DD} arithmetic. */ if (A.Equals3D(B)) return DistancePointSegment(A, C, D); if (C.Equals3D(B)) return DistancePointSegment(C, A, B); /** * Algorithm derived from http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm */ var a = Vector3D.Dot(A, B, A, B); var b = Vector3D.Dot(A, B, C, D); var c = Vector3D.Dot(C, D, C, D); var d = Vector3D.Dot(A, B, C, A); var e = Vector3D.Dot(C, D, C, A); var denom = a * c - b * b; if (Double.IsNaN(denom)) throw new ArgumentException("Ordinates must not be NaN"); double s; double t; if (denom <= 0.0) { /** * The lines are parallel. * In this case solve for the parameters s and t by assuming s is 0. */ s = 0; // choose largest denominator for optimal numeric conditioning if (b > c) t = d / b; else t = e / c; } else { s = (b * e - c * d) / denom; t = (a * e - b * d) / denom; } if (s < 0) return DistancePointSegment(A, C, D); if (s > 1) return DistancePointSegment(B, C, D); if (t < 0) return DistancePointSegment(C, A, B); if (t > 1) { return DistancePointSegment(D, A, B); } /** * The closest points are in interiors of segments, * so compute them directly */ var x1 = A.X + s * (B.X - A.X); var y1 = A.Y + s * (B.Y - A.Y); var z1 = A.Z + s * (B.Z - A.Z); var x2 = C.X + t * (D.X - C.X); var y2 = C.Y + t * (D.Y - C.Y); var z2 = C.Z + t * (D.Z - C.Z); // length (p1-p2) return Distance(new Coordinate(x1, y1, z1), new Coordinate(x2, y2, z2)); }
public void TestClone() { Coordinate c = new Coordinate(100.0, 200.0, 50.0); Coordinate clone = (Coordinate)c.Clone(); Assert.IsTrue(c.Equals3D(clone)); }
public void TestEquals3D() { Coordinate c1 = new Coordinate(1, 2, 3); Coordinate c2 = new Coordinate(1, 2, 3); Assert.IsTrue(c1.Equals3D(c2)); Coordinate c3 = new Coordinate(1, 22, 3); Assert.IsFalse(c1.Equals3D(c3)); }