1/*
2Solving the Nearest Point-on-Curve Problem
3and
4A Bezier Curve-Based Root-Finder
5by Philip J. Schneider
6from "Graphics Gems", Academic Press, 1990
7*/
8
9 /*    point_on_curve.c    */
10
11#include <stdio.h>
12#include <malloc.h>
13#include <math.h>
14#include "GraphicsGems.h"
15
16#define TESTMODE
17
18/*
19 *  Forward declarations
20 */
21Point2  NearestPointOnCurve();
22static    int    FindRoots();
23static    Point2    *ConvertToBezierForm();
24static    double    ComputeXIntercept();
25static    int    ControlPolygonFlatEnough();
26static    int    CrossingCount();
27static    Point2    Bezier();
28static    Vector2    V2ScaleII();
29
30int        MAXDEPTH = 64;    /*  Maximum depth for recursion */
31
32#define    EPSILON    (ldexp(1.0,-MAXDEPTH-1)) /*Flatness control value */
33#define    DEGREE    3            /*  Cubic Bezier curve        */
34#define    W_DEGREE 5            /*  Degree of eqn to find roots of */
35
36#ifdef TESTMODE
37/*
38 *  main :
39 *    Given a cubic Bezier curve (i.e., its control points), and some
40 *    arbitrary point in the plane, find the point on the curve
41 *    closest to that arbitrary point.
42 */
43main()
44{
45
46 static Point2 bezCurve[4] = {    /*  A cubic Bezier curve    */
47    { 0.0, 0.0 },
48    { 1.0, 2.0 },
49    { 3.0, 3.0 },
50    { 4.0, 2.0 },
51    };
52    static Point2 arbPoint = { 3.5, 2.0 }; /*Some arbitrary point*/
53    Point2    pointOnCurve;         /*  Nearest point on the curve */
54
55    /*  Find the closest point */
56    pointOnCurve = NearestPointOnCurve(arbPoint, bezCurve);
57    printf("pointOnCurve : (%4.4f, %4.4f)\n", pointOnCurve.x,
58        pointOnCurve.y);
59}
60#endif /* TESTMODE */
61
62
63/*
64 *  NearestPointOnCurve :
65 *      Compute the parameter value of the point on a Bezier
66 *        curve segment closest to some arbtitrary, user-input point.
67 *        Return the point on the curve at that parameter value.
68 *
69 */
70Point2 NearestPointOnCurve(P, V)
71    Point2     P;            /* The user-supplied point      */
72    Point2     *V;            /* Control points of cubic Bezier */
73{
74    Point2    *w;            /* Ctl pts for 5th-degree eqn    */
75    double     t_candidate[W_DEGREE];    /* Possible roots        */
76    int     n_solutions;        /* Number of roots found    */
77    double    t;            /* Parameter value of closest pt*/
78
79    /*  Convert problem to 5th-degree Bezier form    */
80    w = ConvertToBezierForm(P, V);
81
82    /* Find all possible roots of 5th-degree equation */
83    n_solutions = FindRoots(w, W_DEGREE, t_candidate, 0);
84    free((char *)w);
85
86    /* Compare distances of P to all candidates, and to t=0, and t=1 */
87    {
88        double     dist, new_dist;
89        Point2     p;
90        Vector2  v;
91        int        i;
92
93
94    /* Check distance to beginning of curve, where t = 0    */
95        dist = V2SquaredLength(V2Sub(&P, &V[0], &v));
96            t = 0.0;
97
98    /* Find distances for candidate points    */
99        for (i = 0; i < n_solutions; i++) {
100            p = Bezier(V, DEGREE, t_candidate[i],
101            (Point2 *)NULL, (Point2 *)NULL);
102            new_dist = V2SquaredLength(V2Sub(&P, &p, &v));
103            if (new_dist < dist) {
104                    dist = new_dist;
105                    t = t_candidate[i];
106            }
107        }
108
109    /* Finally, look at distance to end point, where t = 1.0 */
110        new_dist = V2SquaredLength(V2Sub(&P, &V[DEGREE], &v));
111            if (new_dist < dist) {
112                dist = new_dist;
113            t = 1.0;
114        }
115    }
116
117    /*  Return the point on the curve at parameter value t */
118    printf("t : %4.12f\n", t);
119    return (Bezier(V, DEGREE, t, (Point2 *)NULL, (Point2 *)NULL));
120}
121
122
123/*
124 *  ConvertToBezierForm :
125 *        Given a point and a Bezier curve, generate a 5th-degree
126 *        Bezier-format equation whose solution finds the point on the
127 *      curve nearest the user-defined point.
128 */
129static Point2 *ConvertToBezierForm(P, V)
130    Point2     P;            /* The point to find t for    */
131    Point2     *V;            /* The control points        */
132{
133    int     i, j, k, m, n, ub, lb;
134    int     row, column;        /* Table indices        */
135    Vector2     c[DEGREE+1];        /* V(i)'s - P            */
136    Vector2     d[DEGREE];        /* V(i+1) - V(i)        */
137    Point2     *w;            /* Ctl pts of 5th-degree curve  */
138    double     cdTable[3][4];        /* Dot product of c, d        */
139    static double z[3][4] = {    /* Precomputed "z" for cubics    */
140    {1.0, 0.6, 0.3, 0.1},
141    {0.4, 0.6, 0.6, 0.4},
142    {0.1, 0.3, 0.6, 1.0},
143    };
144
145
146    /*Determine the c's -- these are vectors created by subtracting*/
147    /* point P from each of the control points                */
148    for (i = 0; i <= DEGREE; i++) {
149        V2Sub(&V[i], &P, &c[i]);
150    }
151    /* Determine the d's -- these are vectors created by subtracting*/
152    /* each control point from the next                    */
153    for (i = 0; i <= DEGREE - 1; i++) {
154        d[i] = V2ScaleII(V2Sub(&V[i+1], &V[i], &d[i]), 3.0);
155    }
156
157    /* Create the c,d table -- this is a table of dot products of the */
158    /* c's and d's                            */
159    for (row = 0; row <= DEGREE - 1; row++) {
160        for (column = 0; column <= DEGREE; column++) {
161            cdTable[row][column] = V2Dot(&d[row], &c[column]);
162        }
163    }
164
165    /* Now, apply the z's to the dot products, on the skew diagonal*/
166    /* Also, set up the x-values, making these "points"        */
167    w = (Point2 *)malloc((unsigned)(W_DEGREE+1) * sizeof(Point2));
168    for (i = 0; i <= W_DEGREE; i++) {
169        w[i].y = 0.0;
170        w[i].x = (double)(i) / W_DEGREE;
171    }
172
173    n = DEGREE;
174    m = DEGREE-1;
175    for (k = 0; k <= n + m; k++) {
176        lb = MAX(0, k - m);
177        ub = MIN(k, n);
178        for (i = lb; i <= ub; i++) {
179            j = k - i;
180            w[i+j].y += cdTable[j][i] * z[j][i];
181        }
182    }
183
184    return (w);
185}
186
187
188/*
189 *  FindRoots :
190 *    Given a 5th-degree equation in Bernstein-Bezier form, find
191 *    all of the roots in the interval [0, 1].  Return the number
192 *    of roots found.
193 */
194static int FindRoots(w, degree, t, depth)
195    Point2     *w;            /* The control points        */
196    int     degree;        /* The degree of the polynomial    */
197    double     *t;            /* RETURN candidate t-values    */
198    int     depth;        /* The depth of the recursion    */
199{
200    int     i;
201    Point2     Left[W_DEGREE+1],    /* New left and right         */
202              Right[W_DEGREE+1];    /* control polygons        */
203    int     left_count,        /* Solution count from        */
204        right_count;        /* children            */
205    double     left_t[W_DEGREE+1],    /* Solutions from kids        */
206               right_t[W_DEGREE+1];
207
208    switch (CrossingCount(w, degree)) {
209           case 0 : {    /* No solutions here    */
210         return 0;
211    }
212    case 1 : {    /* Unique solution    */
213        /* Stop recursion when the tree is deep enough    */
214        /* if deep enough, return 1 solution at midpoint     */
215        if (depth >= MAXDEPTH) {
216            t[0] = (w[0].x + w[W_DEGREE].x) / 2.0;
217            return 1;
218        }
219        if (ControlPolygonFlatEnough(w, degree)) {
220            t[0] = ComputeXIntercept(w, degree);
221            return 1;
222        }
223        break;
224    }
225}
226
227    /* Otherwise, solve recursively after    */
228    /* subdividing control polygon        */
229    Bezier(w, degree, 0.5, Left, Right);
230    left_count  = FindRoots(Left,  degree, left_t, depth+1);
231    right_count = FindRoots(Right, degree, right_t, depth+1);
232
233
234    /* Gather solutions together    */
235    for (i = 0; i < left_count; i++) {
236        t[i] = left_t[i];
237    }
238    for (i = 0; i < right_count; i++) {
239         t[i+left_count] = right_t[i];
240    }
241
242    /* Send back total number of solutions    */
243    return (left_count+right_count);
244}
245
246
247/*
248 * CrossingCount :
249 *    Count the number of times a Bezier control polygon
250 *    crosses the 0-axis. This number is >= the number of roots.
251 *
252 */
253static int CrossingCount(V, degree)
254    Point2    *V;            /*  Control pts of Bezier curve    */
255    int        degree;            /*  Degreee of Bezier curve     */
256{
257    int     i;
258    int     n_crossings = 0;    /*  Number of zero-crossings    */
259    int        sign, old_sign;        /*  Sign of coefficients    */
260
261    sign = old_sign = SGN(V[0].y);
262    for (i = 1; i <= degree; i++) {
263        sign = SGN(V[i].y);
264        if (sign != old_sign) n_crossings++;
265        old_sign = sign;
266    }
267    return n_crossings;
268}
269
270
271
272/*
273 *  ControlPolygonFlatEnough :
274 *    Check if the control polygon of a Bezier curve is flat enough
275 *    for recursive subdivision to bottom out.
276 *
277 */
278static int ControlPolygonFlatEnough(V, degree)
279    Point2    *V;        /* Control points    */
280    int     degree;        /* Degree of polynomial    */
281{
282    int     i;            /* Index variable        */
283    double     *distance;        /* Distances from pts to line    */
284    double     max_distance_above;    /* maximum of these        */
285    double     max_distance_below;
286    double     error;            /* Precision of root        */
287    double     intercept_1,
288               intercept_2,
289               left_intercept,
290               right_intercept;
291    double     a, b, c;        /* Coefficients of implicit    */
292                        /* eqn for line from V[0]-V[deg]*/
293
294    /* Find the  perpendicular distance        */
295    /* from each interior control point to     */
296    /* line connecting V[0] and V[degree]    */
297    distance = (double *)malloc((unsigned)(degree + 1) *                     sizeof(double));
298    {
299    double    abSquared;
300
301    /* Derive the implicit equation for line connecting first *'
302    /*  and last control points */
303    a = V[0].y - V[degree].y;
304    b = V[degree].x - V[0].x;
305    c = V[0].x * V[degree].y - V[degree].x * V[0].y;
306
307    abSquared = (a * a) + (b * b);
308
309        for (i = 1; i < degree; i++) {
310        /* Compute distance from each of the points to that line    */
311            distance[i] = a * V[i].x + b * V[i].y + c;
312            if (distance[i] > 0.0) {
313                distance[i] = (distance[i] * distance[i]) / abSquared;
314            }
315            if (distance[i] < 0.0) {
316                distance[i] = -((distance[i] * distance[i]) /                         abSquared);
317            }
318        }
319    }
320
321
322    /* Find the largest distance    */
323    max_distance_above = 0.0;
324    max_distance_below = 0.0;
325    for (i = 1; i < degree; i++) {
326        if (distance[i] < 0.0) {
327            max_distance_below = MIN(max_distance_below, distance[i]);
328        };
329        if (distance[i] > 0.0) {
330            max_distance_above = MAX(max_distance_above, distance[i]);
331        }
332    }
333    free((char *)distance);
334
335    {
336    double    det, dInv;
337    double    a1, b1, c1, a2, b2, c2;
338
339    /*  Implicit equation for zero line */
340    a1 = 0.0;
341    b1 = 1.0;
342    c1 = 0.0;
343
344    /*  Implicit equation for "above" line */
345    a2 = a;
346    b2 = b;
347    c2 = c + max_distance_above;
348
349    det = a1 * b2 - a2 * b1;
350    dInv = 1.0/det;
351
352    intercept_1 = (b1 * c2 - b2 * c1) * dInv;
353
354    /*  Implicit equation for "below" line */
355    a2 = a;
356    b2 = b;
357    c2 = c + max_distance_below;
358
359    det = a1 * b2 - a2 * b1;
360    dInv = 1.0/det;
361
362    intercept_2 = (b1 * c2 - b2 * c1) * dInv;
363    }
364
365    /* Compute intercepts of bounding box    */
366    left_intercept = MIN(intercept_1, intercept_2);
367    right_intercept = MAX(intercept_1, intercept_2);
368
369    error = 0.5 * (right_intercept-left_intercept);
370    if (error < EPSILON) {
371        return 1;
372    }
373    else {
374        return 0;
375    }
376}
377
378
379
380/*
381 *  ComputeXIntercept :
382 *    Compute intersection of chord from first control point to last
383 *      with 0-axis.
384 *
385 */
386/* NOTE: "T" and "Y" do not have to be computed, and there are many useless
387 * operations in the following (e.g. "0.0 - 0.0").
388 */
389static double ComputeXIntercept(V, degree)
390    Point2     *V;            /*  Control points    */
391    int        degree;         /*  Degree of curve    */
392{
393    double    XLK, YLK, XNM, YNM, XMK, YMK;
394    double    det, detInv;
395    double    S, T;
396    double    X, Y;
397
398    XLK = 1.0 - 0.0;
399    YLK = 0.0 - 0.0;
400    XNM = V[degree].x - V[0].x;
401    YNM = V[degree].y - V[0].y;
402    XMK = V[0].x - 0.0;
403    YMK = V[0].y - 0.0;
404
405    det = XNM*YLK - YNM*XLK;
406    detInv = 1.0/det;
407
408    S = (XNM*YMK - YNM*XMK) * detInv;
409/*  T = (XLK*YMK - YLK*XMK) * detInv; */
410
411    X = 0.0 + XLK * S;
412/*  Y = 0.0 + YLK * S; */
413
414    return X;
415}
416
417
418/*
419 *  Bezier :
420 *    Evaluate a Bezier curve at a particular parameter value
421 *      Fill in control points for resulting sub-curves if "Left" and
422 *    "Right" are non-null.
423 *
424 */
425static Point2 Bezier(V, degree, t, Left, Right)
426    int     degree;        /* Degree of bezier curve    */
427    Point2     *V;            /* Control pts            */
428    double     t;            /* Parameter value        */
429    Point2     *Left;        /* RETURN left half ctl pts    */
430    Point2     *Right;        /* RETURN right half ctl pts    */
431{
432    int     i, j;        /* Index variables    */
433    Point2     Vtemp[W_DEGREE+1][W_DEGREE+1];
434
435
436    /* Copy control points    */
437    for (j =0; j <= degree; j++) {
438        Vtemp[0][j] = V[j];
439    }
440
441    /* Triangle computation    */
442    for (i = 1; i <= degree; i++) {
443        for (j =0 ; j <= degree - i; j++) {
444            Vtemp[i][j].x =
445                  (1.0 - t) * Vtemp[i-1][j].x + t * Vtemp[i-1][j+1].x;
446            Vtemp[i][j].y =
447                  (1.0 - t) * Vtemp[i-1][j].y + t * Vtemp[i-1][j+1].y;
448        }
449    }
450
451    if (Left != NULL) {
452        for (j = 0; j <= degree; j++) {
453            Left[j]  = Vtemp[j][0];
454        }
455    }
456    if (Right != NULL) {
457        for (j = 0; j <= degree; j++) {
458            Right[j] = Vtemp[degree-j][j];
459        }
460    }
461
462    return (Vtemp[degree][0]);
463}
464
465static Vector2 V2ScaleII(v, s)
466    Vector2    *v;
467    double    s;
468{
469    Vector2 result;
470
471    result.x = v->x * s; result.y = v->y * s;
472    return (result);
473}
474