public void It_Returns_The_Closest_Point_On_A_Segment(double[] ptToCheck, double[] ptExpected, double tValExpected) { // Arrange // We are not passing a segment like a line or ray but the part that compose the segment, // t values [0 and 1] and start and end point. var testPt = new Point3(ptToCheck[0], ptToCheck[1], ptToCheck[2]); var expectedPt = new Point3(ptExpected[0], ptExpected[1], ptExpected[2]); Point3 pt0 = new Point3(5, 5, 0); Point3 pt1 = new Point3(10, 10, 0); // Act (double tValue, Point3 pt)closestPt = Trigonometry.ClosestPointToSegment(testPt, pt0, pt1, 0, 1); // Assert closestPt.tValue.Should().BeApproximately(tValExpected, GSharkMath.MaxTolerance); closestPt.pt.EpsilonEquals(expectedPt, GSharkMath.Epsilon).Should().BeTrue(); }
/// <summary> /// Computes the closest parameters on a curve to a given point.<br/> /// <em>Corresponds to page 244 chapter six from The NURBS Book by Piegl and Tiller.</em> /// </summary> /// <param name="curve">The curve object.</param> /// <param name="point">Point to search from.</param> /// <returns>The closest parameter on the curve.</returns> internal static double ClosestParameter(NurbsBase curve, Point3 point) { double minimumDistance = double.PositiveInfinity; double tParameter = 0D; List <Point3> ctrlPts = curve.ControlPointLocations; (List <double> tValues, List <Point3> pts) = Sampling.Curve.RegularSample(curve, ctrlPts.Count * curve.Degree); for (int i = 0; i < pts.Count - 1; i++) { double t0 = tValues[i]; double t1 = tValues[i + 1]; Point3 pt0 = pts[i]; Point3 pt1 = pts[i + 1]; var(tValue, pt) = Trigonometry.ClosestPointToSegment(point, pt0, pt1, t0, t1); double distance = pt.DistanceTo(point); if (!(distance < minimumDistance)) { continue; } minimumDistance = distance; tParameter = tValue; } int maxIterations = 5; int j = 0; // Two zero tolerances can be used to indicate convergence: double tol1 = GSharkMath.MaxTolerance; // a measure of Euclidean distance; double tol2 = 0.0005; // a zero cosine measure. double tVal0 = curve.Knots[0]; double tVal1 = curve.Knots[curve.Knots.Count - 1]; bool closedCurve = (ctrlPts[0] - ctrlPts[ctrlPts.Count - 1]).SquareLength < GSharkMath.Epsilon; double Cu = tParameter; // To avoid infinite loop we limited the interaction. while (j < maxIterations) { List <Vector3> e = Evaluate.Curve.RationalDerivatives(curve, Cu, 2); Vector3 diff = e[0] - new Vector3(point); // C(u) - P // First condition, point coincidence: // |C(u) - p| < e1 double c1v = diff.Length; bool c1 = c1v <= tol1; // Second condition, zero cosine: // C'(u) * (C(u) - P) // ------------------ < e2 // |C'(u)| |C(u) - P| double c2n = Vector3.DotProduct(e[1], diff); double c2d = (e[1] * c1v).Length; double c2v = c2n / c2d; bool c2 = Math.Abs(c2v) <= tol2; // If at least one of these conditions is not satisfied, // a new value, ui+l> is computed using the NewtonIteration. // Then two more conditions are checked. if (c1 && c2) { return(Cu); } double ct = NewtonIteration(Cu, e, diff); // Ensure that the parameter stays within the boundary of the curve. if (ct < tVal0) { ct = closedCurve ? tVal1 - (ct - tVal0) : tVal0; } if (ct > tVal1) { ct = closedCurve ? tVal0 + (ct - tVal1) : tVal1; } // the parameter does not change significantly, the point is off the end of the curve. double c3v = (e[1] * (ct - Cu)).Length; if (c3v < tol1) { return(Cu); } Cu = ct; j++; } return(Cu); }