1 /* 2 Solving the Nearest Point-on-Curve Problem 3 and 4 A Bezier Curve-Based Root-Finder 5 by Philip J. Schneider 6 from "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 */ 21 Point2 NearestPointOnCurve(); 22 static int FindRoots(); 23 static Point2 *ConvertToBezierForm(); 24 static double ComputeXIntercept(); 25 static int ControlPolygonFlatEnough(); 26 static int CrossingCount(); 27 static Point2 Bezier(); 28 static Vector2 V2ScaleII(); 29 30 int 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 */ 43 main() 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 */ 70 Point2 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 */ 129 static 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 */ 194 static 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 */ 253 static 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 */ 278 static 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 */ 389 static 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 */ 425 static 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 465 static 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