nearestpoint.cpp 16.2 KB
 Till Theato committed Feb 01, 2011 1 2 3 4 5 6 7 8 9 10 11 ``````/* 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 `````` Dan Dennedy committed Feb 15, 2011 12 ``````#include `````` Till Theato committed Feb 01, 2011 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 ``````#include #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 */ `````` Vincent PINON committed May 12, 2014 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 `````` 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]; `````` Till Theato committed Feb 01, 2011 122 `````` } `````` Vincent PINON committed May 12, 2014 123 `````` } `````` Till Theato committed Feb 01, 2011 124 `````` `````` Vincent PINON committed May 12, 2014 125 126 127 128 `````` /* Finally, look at distance to end point, where t = 1.0 */ new_dist = V2SquaredLength(V2Sub(&P, &V[DEGREE], &v)); if (new_dist < dist) { t = 1.0; `````` Till Theato committed Feb 01, 2011 129 130 131 `````` } /* Return the point on the curve at parameter value t */ `````` Vincent PINON committed May 12, 2014 132 `````` // printf("t : %4.12f\n", t); `````` Till Theato committed Feb 01, 2011 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 `````` *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 */ `````` Laurent Montel committed May 14, 2013 163 `````` for (i = 0; i <= DEGREE; ++i) { `````` Till Theato committed Feb 01, 2011 164 165 166 167 `````` V2Sub(&V[i], &P, &c[i]); } /* Determine the d's -- these are vectors created by subtracting*/ /* each control point from the next */ `````` Laurent Montel committed May 14, 2013 168 `````` for (i = 0; i <= DEGREE - 1; ++i) { `````` Till Theato committed Feb 01, 2011 169 170 171 172 173 174 175 176 177 178 179 180 181 182 `````` 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)); `````` Laurent Montel committed May 14, 2013 183 `````` for (i = 0; i <= W_DEGREE; ++i) { `````` Till Theato committed Feb 01, 2011 184 185 186 187 188 189 190 191 192 `````` 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); `````` Laurent Montel committed May 14, 2013 193 `````` for (i = lb; i <= ub; ++i) { `````` Till Theato committed Feb 01, 2011 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 `````` 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 */ `````` Vincent PINON committed May 12, 2014 221 `````` right_t[W_DEGREE+1]; `````` Till Theato committed Feb 01, 2011 222 223 `````` switch (CrossingCount(w, degree)) { `````` Vincent PINON committed May 12, 2014 224 225 226 227 `````` case 0 : /* No solutions here */ return 0; case 1 : /* Unique solution */ `````` Till Theato committed Feb 01, 2011 228 229 230 `````` /* Stop recursion when the tree is deep enough */ /* if deep enough, return 1 solution at midpoint */ if (depth >= MAXDEPTH) { `````` Vincent PINON committed May 12, 2014 231 232 `````` t[0] = (w[0].x + w[W_DEGREE].x) / 2.0; return 1; `````` Till Theato committed Feb 01, 2011 233 234 `````` } if (ControlPolygonFlatEnough(w, degree)) { `````` Vincent PINON committed May 12, 2014 235 236 `````` t[0] = ComputeXIntercept(w, degree); return 1; `````` Till Theato committed Feb 01, 2011 237 238 `````` } break; `````` Vincent PINON committed May 12, 2014 239 `````` } `````` Till Theato committed Feb 01, 2011 240 241 242 243 244 245 246 247 248 `````` /* 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 */ `````` Laurent Montel committed May 14, 2013 249 `````` for (i = 0; i < left_count; ++i) { `````` Vincent PINON committed May 12, 2014 250 `````` t[i] = left_t[i]; /* FIXME: static analyzer says left_t can be undefined */ `````` Till Theato committed Feb 01, 2011 251 `````` } `````` Laurent Montel committed May 14, 2013 252 `````` for (i = 0; i < right_count; ++i) { `````` Vincent PINON committed May 12, 2014 253 `````` t[i+left_count] = right_t[i]; /* FIXME: static analyzer says right_t can be undefined */ `````` Till Theato committed Feb 01, 2011 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 `````` } /* 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 */ `````` Vincent PINON committed May 12, 2014 275 `````` old_sign = SGN(V[0].y); `````` Laurent Montel committed May 14, 2013 276 `````` for (i = 1; i <= degree; ++i) { `````` Till Theato committed Feb 01, 2011 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 `````` 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; `````` Laurent Montel committed May 14, 2013 358 `````` for (i = 1; i < degree; ++i) `````` Till Theato committed Feb 01, 2011 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 `````` { 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 */ `````` Laurent Montel committed May 14, 2013 469 `````` for (i = 1; i <= degree; ++i) { `````` Till Theato committed Feb 01, 2011 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 `````` 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); }``````