/*Detailed Computational Costs Comparisons Div Mult Additions Assignments splineGuess log(n,2) 2 3 1 (n currently set to 100) Newton Iter f 21 20 13 df 20 19 11 update 4 1 3 6 total 4 1 41 42 30 \endverbatim To evaluate u to SimTK::Eps*100 this typically involves 2 Newton iterations, yielding a total cost of \verbatim Comparisons Div Mult Additions Assignments eval U 7+8=15 2 82 42 60 */ double SegmentedQuinticBezierToolkit::calcU(double ax, const SimTK::Vector& bezierPtsX, const SimTK::Spline& splineUX, double tol, int maxIter) { //Check to make sure that ax is in the curve domain double minX = 1e100; double maxX = -1e100; for(int i=0; i<bezierPtsX.nrow(); i++){ if(bezierPtsX(i) > maxX) maxX = bezierPtsX(i); if(bezierPtsX(i) < minX) minX = bezierPtsX(i); } SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, "SegmentedQuinticBezierToolkit::calcU", "Error: input ax was not in the domain of the Bezier curve specified \n" "by the control points in bezierPtsX."); double u = splineUX.calcValue(ax); u = clampU(u); double f = 0; f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; double df= 0; double du= 0; int iter = 0; bool pathologic = false; //Newton iterate to the desired tolerance while(abs(f) > tol && iter < maxIter && pathologic == false){ //Take a Newton step df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1); if(abs(df) > 0){ du = -f/df; u = u + du; u = clampU(u); f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; }else{ pathologic = true; } iter++; } //Check for convergence SimTK_ERRCHK2_ALWAYS( (f <= tol), "SegmentedQuinticBezierToolkit::calcU", "Error: desired tolerance of %f on U not met by the Newton iteration." " A tolerance of %f was reached.",tol, f); SimTK_ERRCHK_ALWAYS( (pathologic==false), "SegmentedQuinticBezierToolkit::calcU", "Error: Newton iteration went pathologic: df = 0 to machine precision."); //Return the value return u; }