NearestPoint.cpp revision 7e0274e80bf87a923cb56f7febb44be36bd8b987 
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