git.sesse.net Git - kdenlive/blob - src/onmonitoritems/rotoscoping/nearestpoint.cpp

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