+/*
+Solving the Nearest Point-on-Curve Problem
+and
+A Bezier Curve-Based Root-Finder
+by Philip J. Schneider
+from "Graphics Gems", Academic Press, 1990
+*/
+
+ /* point_on_curve.c */
+
+#include <stdio.h>
+#include <malloc.h>
+#include <math.h>
+
+#include "nearestpoint.h"
+
+//#define TESTMODE
+
+/*
+ * Copied from ggveclib.c to avoid externs
+ * Copyright:
+2d and 3d Vector C Library
+by Andrew Glassner
+from "Graphics Gems", Academic Press, 1990
+ */
+
+/* returns squared length of input vector */
+double V2SquaredLength(Vector2 *a) { return((a->x * a->x)+(a->y * a->y)); }
+/* return the dot product of vectors a and b */
+double V2Dot(Vector2 *a, Vector2 *b) { return((a->x*b->x)+(a->y*b->y)); }
+/* return vector difference c = a-b */
+Vector2 *V2Sub(Vector2 *a, Vector2 *b, Vector2 *c) { c->x = a->x-b->x; c->y = a->y-b->y; return(c); }
+
+
+
+/*
+ * Forward declarations
+ */
+Point2 NearestPointOnCurve(Point2 P, Point2 *V);
+static int FindRoots(Point2 *w, int degree, double *t, int depth);
+static Point2 *ConvertToBezierForm(Point2 P, Point2 *V);
+static double ComputeXIntercept(Point2 *V, int degree);
+static int ControlPolygonFlatEnough(Point2 *V, int degree);
+static int CrossingCount(Point2 *V, int degree);
+static Point2 Bezier(Point2 *V, int degree, double t, Point2 *Left, Point2 *Right);
+static Vector2 V2ScaleII(Point2 *v, double s);
+
+int MAXDEPTH = 64; /* Maximum depth for recursion */
+
+#define EPSILON (ldexp(1.0,-MAXDEPTH-1)) /*Flatness control value */
+#define DEGREE 3 /* Cubic Bezier curve */
+#define W_DEGREE 5 /* Degree of eqn to find roots of */
+
+#ifdef TESTMODE
+/*
+ * main :
+ * Given a cubic Bezier curve (i.e., its control points), and some
+ * arbitrary point in the plane, find the point on the curve
+ * closest to that arbitrary point.
+ */
+main()
+{
+
+ static Point2 bezCurve[4] = { /* A cubic Bezier curve */
+ { 0.0, 0.0 },
+ { 1.0, 2.0 },
+ { 3.0, 3.0 },
+ { 4.0, 2.0 },
+ };
+ static Point2 arbPoint = { 3.5, 2.0 }; /*Some arbitrary point*/
+ Point2 pointOnCurve; /* Nearest point on the curve */
+
+ /* Find the closest point */
+ pointOnCurve = NearestPointOnCurve(arbPoint, bezCurve);
+ printf("pointOnCurve : (%4.4f, %4.4f)\n", pointOnCurve.x,
+ pointOnCurve.y);
+}
+#endif /* TESTMODE */
+
+
+/*
+ * NearestPointOnCurve :
+ * Compute the parameter value of the point on a Bezier
+ * curve segment closest to some arbtitrary, user-input point.
+ * Return the point on the curve at that parameter value.
+ *
+ */
+Point2 NearestPointOnCurve(Point2 P, Point2 *V, double *tOut)
+// Point2 P; /* The user-supplied point */
+// Point2 *V; /* Control points of cubic Bezier */
+{
+ Point2 *w; /* Ctl pts for 5th-degree eqn */
+ double t_candidate[W_DEGREE]; /* Possible roots */
+ int n_solutions; /* Number of roots found */
+ double t; /* Parameter value of closest pt*/
+
+ /* Convert problem to 5th-degree Bezier form */
+ w = ConvertToBezierForm(P, V);
+
+ /* Find all possible roots of 5th-degree equation */
+ n_solutions = FindRoots(w, W_DEGREE, t_candidate, 0);
+ free((char *)w);
+
+ /* Compare distances of P to all candidates, and to t=0, and t=1 */
+ {
+ double dist, new_dist;
+ Point2 p;
+ Vector2 v;
+ int i;
+
+
+ /* Check distance to beginning of curve, where t = 0 */
+ dist = V2SquaredLength(V2Sub(&P, &V[0], &v));
+ t = 0.0;
+
+ /* Find distances for candidate points */
+ for (i = 0; i < n_solutions; i++) {
+ p = Bezier(V, DEGREE, t_candidate[i],
+ (Point2 *)NULL, (Point2 *)NULL);
+ new_dist = V2SquaredLength(V2Sub(&P, &p, &v));
+ if (new_dist < dist) {
+ dist = new_dist;
+ t = t_candidate[i];
+ }
+ }
+
+ /* Finally, look at distance to end point, where t = 1.0 */
+ new_dist = V2SquaredLength(V2Sub(&P, &V[DEGREE], &v));
+ if (new_dist < dist) {
+ dist = new_dist;
+ t = 1.0;
+ }
+ }
+
+ /* Return the point on the curve at parameter value t */
+// printf("t : %4.12f\n", t);
+ *tOut = t;
+ return (Bezier(V, DEGREE, t, (Point2 *)NULL, (Point2 *)NULL));
+}
+
+
+/*
+ * ConvertToBezierForm :
+ * Given a point and a Bezier curve, generate a 5th-degree
+ * Bezier-format equation whose solution finds the point on the
+ * curve nearest the user-defined point.
+ */
+static Point2 *ConvertToBezierForm(Point2 P, Point2 *V)
+// Point2 P; /* The point to find t for */
+// Point2 *V; /* The control points */
+{
+ int i, j, k, m, n, ub, lb;
+ int row, column; /* Table indices */
+ Vector2 c[DEGREE+1]; /* V(i)'s - P */
+ Vector2 d[DEGREE]; /* V(i+1) - V(i) */
+ Point2 *w; /* Ctl pts of 5th-degree curve */
+ double cdTable[3][4]; /* Dot product of c, d */
+ static double z[3][4] = { /* Precomputed "z" for cubics */
+ {1.0, 0.6, 0.3, 0.1},
+ {0.4, 0.6, 0.6, 0.4},
+ {0.1, 0.3, 0.6, 1.0},
+ };
+
+
+ /*Determine the c's -- these are vectors created by subtracting*/
+ /* point P from each of the control points */
+ for (i = 0; i <= DEGREE; i++) {
+ V2Sub(&V[i], &P, &c[i]);
+ }
+ /* Determine the d's -- these are vectors created by subtracting*/
+ /* each control point from the next */
+ for (i = 0; i <= DEGREE - 1; i++) {
+ d[i] = V2ScaleII(V2Sub(&V[i+1], &V[i], &d[i]), 3.0);
+ }
+
+ /* Create the c,d table -- this is a table of dot products of the */
+ /* c's and d's */
+ for (row = 0; row <= DEGREE - 1; row++) {
+ for (column = 0; column <= DEGREE; column++) {
+ cdTable[row][column] = V2Dot(&d[row], &c[column]);
+ }
+ }
+
+ /* Now, apply the z's to the dot products, on the skew diagonal*/
+ /* Also, set up the x-values, making these "points" */
+ w = (Point2 *)malloc((unsigned)(W_DEGREE+1) * sizeof(Point2));
+ for (i = 0; i <= W_DEGREE; i++) {
+ w[i].y = 0.0;
+ w[i].x = (double)(i) / W_DEGREE;
+ }
+
+ n = DEGREE;
+ m = DEGREE-1;
+ for (k = 0; k <= n + m; k++) {
+ lb = MAX(0, k - m);
+ ub = MIN(k, n);
+ for (i = lb; i <= ub; i++) {
+ j = k - i;
+ w[i+j].y += cdTable[j][i] * z[j][i];
+ }
+ }
+
+ return (w);
+}
+
+
+/*
+ * FindRoots :
+ * Given a 5th-degree equation in Bernstein-Bezier form, find
+ * all of the roots in the interval [0, 1]. Return the number
+ * of roots found.
+ */
+static int FindRoots(Point2 *w, int degree, double *t, int depth)
+// Point2 *w; /* The control points */
+// int degree; /* The degree of the polynomial */
+// double *t; /* RETURN candidate t-values */
+// int depth; /* The depth of the recursion */
+{
+ int i;
+ Point2 Left[W_DEGREE+1], /* New left and right */
+ Right[W_DEGREE+1]; /* control polygons */
+ int left_count, /* Solution count from */
+ right_count; /* children */
+ double left_t[W_DEGREE+1], /* Solutions from kids */
+ right_t[W_DEGREE+1];
+
+ switch (CrossingCount(w, degree)) {
+ case 0 : { /* No solutions here */
+ return 0;
+ }
+ case 1 : { /* Unique solution */
+ /* Stop recursion when the tree is deep enough */
+ /* if deep enough, return 1 solution at midpoint */
+ if (depth >= MAXDEPTH) {
+ t[0] = (w[0].x + w[W_DEGREE].x) / 2.0;
+ return 1;
+ }
+ if (ControlPolygonFlatEnough(w, degree)) {
+ t[0] = ComputeXIntercept(w, degree);
+ return 1;
+ }
+ break;
+ }
+}
+
+ /* Otherwise, solve recursively after */
+ /* subdividing control polygon */
+ Bezier(w, degree, 0.5, Left, Right);
+ left_count = FindRoots(Left, degree, left_t, depth+1);
+ right_count = FindRoots(Right, degree, right_t, depth+1);
+
+
+ /* Gather solutions together */
+ for (i = 0; i < left_count; i++) {
+ t[i] = left_t[i];
+ }
+ for (i = 0; i < right_count; i++) {
+ t[i+left_count] = right_t[i];
+ }
+
+ /* Send back total number of solutions */
+ return (left_count+right_count);
+}
+
+
+/*
+ * CrossingCount :
+ * Count the number of times a Bezier control polygon
+ * crosses the 0-axis. This number is >= the number of roots.
+ *
+ */
+static int CrossingCount(Point2 *V, int degree)
+// Point2 *V; /* Control pts of Bezier curve */
+// int degree; /* Degreee of Bezier curve */
+{
+ int i;
+ int n_crossings = 0; /* Number of zero-crossings */
+ int sign, old_sign; /* Sign of coefficients */
+
+ sign = old_sign = SGN(V[0].y);
+ for (i = 1; i <= degree; i++) {
+ sign = SGN(V[i].y);
+ if (sign != old_sign) n_crossings++;
+ old_sign = sign;
+ }
+ return n_crossings;
+}
+
+
+
+/*
+ * ControlPolygonFlatEnough :
+ * Check if the control polygon of a Bezier curve is flat enough
+ * for recursive subdivision to bottom out.
+ *
+ * Corrections by James Walker, jw@jwwalker.com, as follows:
+
+There seem to be errors in the ControlPolygonFlatEnough function in the
+Graphics Gems book and the repository (NearestPoint.c). This function
+is briefly described on p. 413 of the text, and appears on pages 793-794.
+I see two main problems with it.
+
+The idea is to find an upper bound for the error of approximating the x
+intercept of the Bezier curve by the x intercept of the line through the
+first and last control points. It is claimed on p. 413 that this error is
+bounded by half of the difference between the intercepts of the bounding
+box. I don't see why that should be true. The line joining the first and
+last control points can be on one side of the bounding box, and the actual
+curve can be near the opposite side, so the bound should be the difference
+of the bounding box intercepts, not half of it.
+
+Second, we come to the implementation. The values distance[i] computed in
+the first loop are not actual distances, but squares of distances. I
+realize that minimizing or maximizing the squares is equivalent to
+minimizing or maximizing the distances. But when the code claims that
+one of the sides of the bounding box has equation
+a * x + b * y + c + max_distance_above, where max_distance_above is one of
+those squared distances, that makes no sense to me.
+
+I have appended my version of the function. If you apply my code to the
+cubic Bezier curve used to test NearestPoint.c,
+
+ static Point2 bezCurve[4] = { / A cubic Bezier curve /
+ { 0.0, 0.0 },
+ { 1.0, 2.0 },
+ { 3.0, 3.0 },
+ { 4.0, 2.0 },
+ };
+
+my code computes left_intercept = -3.0 and right_intercept = 0.0, which you
+can verify by sketching a graph. The original code computes
+left_intercept = 0.0 and right_intercept = 0.9.
+
+ */
+
+/* static int ControlPolygonFlatEnough( const Point2* V, int degree ) */
+static int ControlPolygonFlatEnough(Point2 *V, int degree)
+// Point2 *V; /* Control points */
+// int degree; /* Degree of polynomial */
+{
+ int i; /* Index variable */
+ double value;
+ double max_distance_above;
+ double max_distance_below;
+ double error; /* Precision of root */
+ double intercept_1,
+ intercept_2,
+ left_intercept,
+ right_intercept;
+ double a, b, c; /* Coefficients of implicit */
+ /* eqn for line from V[0]-V[deg]*/
+ double det, dInv;
+ double a1, b1, c1, a2, b2, c2;
+
+ /* Derive the implicit equation for line connecting first *'
+ / * and last control points */
+ a = V[0].y - V[degree].y;
+ b = V[degree].x - V[0].x;
+ c = V[0].x * V[degree].y - V[degree].x * V[0].y;
+
+ max_distance_above = max_distance_below = 0.0;
+
+ for (i = 1; i < degree; i++)
+ {
+ value = a * V[i].x + b * V[i].y + c;
+
+ if (value > max_distance_above)
+ {
+ max_distance_above = value;
+ }
+ else if (value < max_distance_below)
+ {
+ max_distance_below = value;
+ }
+ }
+
+ /* Implicit equation for zero line */
+ a1 = 0.0;
+ b1 = 1.0;
+ c1 = 0.0;
+
+ /* Implicit equation for "above" line */
+ a2 = a;
+ b2 = b;
+ c2 = c - max_distance_above;
+
+ det = a1 * b2 - a2 * b1;
+ dInv = 1.0/det;
+
+ intercept_1 = (b1 * c2 - b2 * c1) * dInv;
+
+ /* Implicit equation for "below" line */
+ a2 = a;
+ b2 = b;
+ c2 = c - max_distance_below;
+
+ det = a1 * b2 - a2 * b1;
+ dInv = 1.0/det;
+
+ intercept_2 = (b1 * c2 - b2 * c1) * dInv;
+
+ /* Compute intercepts of bounding box */
+ left_intercept = MIN(intercept_1, intercept_2);
+ right_intercept = MAX(intercept_1, intercept_2);
+
+ error = right_intercept - left_intercept;
+
+ return (error < EPSILON)? 1 : 0;
+}
+
+
+/*
+ * ComputeXIntercept :
+ * Compute intersection of chord from first control point to last
+ * with 0-axis.
+ *
+ */
+/* NOTE: "T" and "Y" do not have to be computed, and there are many useless
+ * operations in the following (e.g. "0.0 - 0.0").
+ */
+static double ComputeXIntercept(Point2 *V, int degree)
+// Point2 *V; /* Control points */
+// int degree; /* Degree of curve */
+{
+ double XLK, YLK, XNM, YNM, XMK, YMK;
+ double det, detInv;
+ double S;//, T;
+ double X;//, Y;
+
+ XLK = 1.0 - 0.0;
+ YLK = 0.0 - 0.0;
+ XNM = V[degree].x - V[0].x;
+ YNM = V[degree].y - V[0].y;
+ XMK = V[0].x - 0.0;
+ YMK = V[0].y - 0.0;
+
+ det = XNM*YLK - YNM*XLK;
+ detInv = 1.0/det;
+
+ S = (XNM*YMK - YNM*XMK) * detInv;
+/* T = (XLK*YMK - YLK*XMK) * detInv; */
+
+ X = 0.0 + XLK * S;
+/* Y = 0.0 + YLK * S; */
+
+ return X;
+}
+
+
+/*
+ * Bezier :
+ * Evaluate a Bezier curve at a particular parameter value
+ * Fill in control points for resulting sub-curves if "Left" and
+ * "Right" are non-null.
+ *
+ */
+static Point2 Bezier(Point2 *V, int degree, double t, Point2 *Left, Point2 *Right)
+// int degree; /* Degree of bezier curve */
+// Point2 *V; /* Control pts */
+// double t; /* Parameter value */
+// Point2 *Left; /* RETURN left half ctl pts */
+// Point2 *Right; /* RETURN right half ctl pts */
+{
+ int i, j; /* Index variables */
+ Point2 Vtemp[W_DEGREE+1][W_DEGREE+1];
+
+
+ /* Copy control points */
+ for (j =0; j <= degree; j++) {
+ Vtemp[0][j] = V[j];
+ }
+
+ /* Triangle computation */
+ for (i = 1; i <= degree; i++) {
+ for (j =0 ; j <= degree - i; j++) {
+ Vtemp[i][j].x =
+ (1.0 - t) * Vtemp[i-1][j].x + t * Vtemp[i-1][j+1].x;
+ Vtemp[i][j].y =
+ (1.0 - t) * Vtemp[i-1][j].y + t * Vtemp[i-1][j+1].y;
+ }
+ }
+
+ if (Left != NULL) {
+ for (j = 0; j <= degree; j++) {
+ Left[j] = Vtemp[j][0];
+ }
+ }
+ if (Right != NULL) {
+ for (j = 0; j <= degree; j++) {
+ Right[j] = Vtemp[degree-j][j];
+ }
+ }
+
+ return (Vtemp[degree][0]);
+}
+
+static Vector2 V2ScaleII(Point2 *v, double s)
+// Vector2 *v;
+// double s;
+{
+ Vector2 result;
+
+ result.x = v->x * s; result.y = v->y * s;
+ return (result);
+}