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