src/2geom/recursive-bezier-intersection.cpp

   1
   2
   3
   4 #include <2geom/basic-intersection.h>
   5 #include <2geom/sbasis-to-bezier.h>
   6 #include <2geom/exception.h>
   7
   8
   9 #include <gsl/gsl_vector.h>
  10 #include <gsl/gsl_multiroots.h>
  11
  12
  13 unsigned intersect_steps = 0;
  14
  15 using std::vector;
  16 namespace Geom {
  17
  18 class OldBezier {
  19 public:
  20     std::vector<Geom::Point> p;
  21     OldBezier() {
  22     }
  23     void split(double t, OldBezier &a, OldBezier &b) const;
  24     Point operator()(double t) const;
  25
  26     ~OldBezier() {}
  27
  28     void bounds(double &minax, double &maxax,
  29                 double &minay, double &maxay) {
  30         // Compute bounding box for a
  31         minax = p[0][X];         // These are the most likely to be extremal
  32         maxax = p.back()[X];
  33         if( minax > maxax )
  34             std::swap(minax, maxax);
  35         for(unsigned i = 1; i < p.size()-1; i++) {
  36             if( p[i][X] < minax )
  37                 minax = p[i][X];
  38             else if( p[i][X] > maxax )
  39                 maxax = p[i][X];
  40         }
  41
  42         minay = p[0][Y];         // These are the most likely to be extremal
  43         maxay = p.back()[Y];
  44         if( minay > maxay )
  45             std::swap(minay, maxay);
  46         for(unsigned i = 1; i < p.size()-1; i++) {
  47             if( p[i][Y] < minay )
  48                 minay = p[i][Y];
  49             else if( p[i][Y] > maxay )
  50                 maxay = p[i][Y];
  51         }
  52
  53     }
  54
  55 };
  56
  57 static void
  58 find_intersections_bezier_recursive(std::vector<std::pair<double, double> > & xs,
  59                    OldBezier a,
  60                    OldBezier b);
  61
  62 void
  63 find_intersections_bezier_recursive( std::vector<std::pair<double, double> > &xs,
  64                     vector<Geom::Point> const & A,
  65                     vector<Geom::Point> const & B,
  66                     double precision) {
  67     OldBezier a, b;
  68     a.p = A;
  69     b.p = B;
  70     return find_intersections_bezier_recursive(xs, a,b);
  71 }
  72
  73
  74 /* The value of 1.0 / (1L<<14) is enough for most applications */
  75 const double INV_EPS = (1L<<14);
  76
  77 /*
  78  * split the curve at the midpoint, returning an array with the two parts
  79  * Temporary storage is minimized by using part of the storage for the result
  80  * to hold an intermediate value until it is no longer needed.
  81  */
  82 void OldBezier::split(double t, OldBezier &left, OldBezier &right) const {
  83     const unsigned sz = p.size();
  84     Geom::Point Vtemp[sz][sz];
  85
  86     /* Copy control points      */
  87     std::copy(p.begin(), p.end(), Vtemp[0]);
  88
  89     /* Triangle computation     */
  90     for (unsigned i = 1; i < sz; i++) {
  91         for (unsigned j = 0; j < sz - i; j++) {
  92             Vtemp[i][j] = lerp(t, Vtemp[i-1][j], Vtemp[i-1][j+1]);
  93         }
  94     }
  95
  96     left.p.resize(sz);
  97     right.p.resize(sz);
  98     for (unsigned j = 0; j < sz; j++)
  99         left.p[j]  = Vtemp[j][0];
 100     for (unsigned j = 0; j < sz; j++)
 101         right.p[j] = Vtemp[sz-1-j][j];
 102 }
 103
 104 #if 0
 105 /*
 106  * split the curve at the midpoint, returning an array with the two parts
 107  * Temporary storage is minimized by using part of the storage for the result
 108  * to hold an intermediate value until it is no longer needed.
 109  */
 110 Point OldBezier::operator()(double t) const {
 111     const unsigned sz = p.size();
 112     Geom::Point Vtemp[sz][sz];
 113
 114     /* Copy control points      */
 115     std::copy(p.begin(), p.end(), Vtemp[0]);
 116
 117     /* Triangle computation     */
 118     for (unsigned i = 1; i < sz; i++) {
 119         for (unsigned j = 0; j < sz - i; j++) {
 120             Vtemp[i][j] = lerp(t, Vtemp[i-1][j], Vtemp[i-1][j+1]);
 121         }
 122     }
 123     return Vtemp[sz-1][0];
 124 }
 125 #endif
 126
 127 // suggested by Sederberg.
 128 Point OldBezier::operator()(double t) const {
 129     int n = p.size()-1;
 130     double u, bc, tn, tmp;
 131     int i;
 132     Point r;
 133     for(int dim = 0; dim < 2; dim++) {
 134         u = 1.0 - t;
 135         bc = 1;
 136         tn = 1;
 137         tmp = p[0][dim]*u;
 138         for(i=1; i<n; i++){
 139             tn = tn*t;
 140             bc = bc*(n-i+1)/i;
 141             tmp = (tmp + tn*bc*p[i][dim])*u;
 142         }
 143         r[dim] = (tmp + tn*t*p[n][dim]);
 144     }
 145     return r;
 146 }
 147
 148
 149 /*
 150  * Test the bounding boxes of two OldBezier curves for interference.
 151  * Several observations:
 152  *      First, it is cheaper to compute the bounding box of the second curve
 153  *      and test its bounding box for interference than to use a more direct
 154  *      approach of comparing all control points of the second curve with
 155  *      the various edges of the bounding box of the first curve to test
 156  *      for interference.
 157  *      Second, after a few subdivisions it is highly probable that two corners
 158  *      of the bounding box of a given Bezier curve are the first and last
 159  *      control point.  Once this happens once, it happens for all subsequent
 160  *      subcurves.  It might be worth putting in a test and then short-circuit
 161  *      code for further subdivision levels.
 162  *      Third, in the final comparison (the interference test) the comparisons
 163  *      should both permit equality.  We want to find intersections even if they
 164  *      occur at the ends of segments.
 165  *      Finally, there are tighter bounding boxes that can be derived. It isn't
 166  *      clear whether the higher probability of rejection (and hence fewer
 167  *      subdivisions and tests) is worth the extra work.
 168  */
 169
 170 bool intersect_BB( OldBezier a, OldBezier b ) {
 171     double minax, maxax, minay, maxay;
 172     a.bounds(minax, maxax, minay, maxay);
 173     double minbx, maxbx, minby, maxby;
 174     b.bounds(minbx, maxbx, minby, maxby);
 175     // Test bounding box of b against bounding box of a
 176     // Not >= : need boundary case
 177     return not( ( minax > maxbx ) || ( minay > maxby )
 178                 || ( minbx > maxax ) || ( minby > maxay ) );
 179 }
 180
 181 /*
 182  * Recursively intersect two curves keeping track of their real parameters
 183  * and depths of intersection.
 184  * The results are returned in a 2-D array of doubles indicating the parameters
 185  * for which intersections are found.  The parameters are in the order the
 186  * intersections were found, which is probably not in sorted order.
 187  * When an intersection is found, the parameter value for each of the two
 188  * is stored in the index elements array, and the index is incremented.
 189  *
 190  * If either of the curves has subdivisions left before it is straight
 191  *      (depth > 0)
 192  * that curve (possibly both) is (are) subdivided at its (their) midpoint(s).
 193  * the depth(s) is (are) decremented, and the parameter value(s) corresponding
 194  * to the midpoints(s) is (are) computed.
 195  * Then each of the subcurves of one curve is intersected with each of the
 196  * subcurves of the other curve, first by testing the bounding boxes for
 197  * interference.  If there is any bounding box interference, the corresponding
 198  * subcurves are recursively intersected.
 199  *
 200  * If neither curve has subdivisions left, the line segments from the first
 201  * to last control point of each segment are intersected.  (Actually the
 202  * only the parameter value corresponding to the intersection point is found).
 203  *
 204  * The apriori flatness test is probably more efficient than testing at each
 205  * level of recursion, although a test after three or four levels would
 206  * probably be worthwhile, since many curves become flat faster than their
 207  * asymptotic rate for the first few levels of recursion.
 208  *
 209  * The bounding box test fails much more frequently than it succeeds, providing
 210  * substantial pruning of the search space.
 211  *
 212  * Each (sub)curve is subdivided only once, hence it is not possible that for
 213  * one final line intersection test the subdivision was at one level, while
 214  * for another final line intersection test the subdivision (of the same curve)
 215  * was at another.  Since the line segments share endpoints, the intersection
 216  * is robust: a near-tangential intersection will yield zero or two
 217  * intersections.
 218  */
 219 void recursively_intersect( OldBezier a, double t0, double t1, int deptha,
 220                            OldBezier b, double u0, double u1, int depthb,
 221                            std::vector<std::pair<double, double> > &parameters)
 222 {
 223     intersect_steps ++;
 224     //std::cout << deptha << std::endl;
 225     if( deptha > 0 )
 226     {
 227         OldBezier A[2];
 228         a.split(0.5, A[0], A[1]);
 229         double tmid = (t0+t1)*0.5;
 230         deptha--;
 231         if( depthb > 0 )
 232         {
 233             OldBezier B[2];
 234             b.split(0.5, B[0], B[1]);
 235             double umid = (u0+u1)*0.5;
 236             depthb--;
 237             if( intersect_BB( A[0], B[0] ) )
 238                 recursively_intersect( A[0], t0, tmid, deptha,
 239                                       B[0], u0, umid, depthb,
 240                                       parameters );
 241             if( intersect_BB( A[1], B[0] ) )
 242                 recursively_intersect( A[1], tmid, t1, deptha,
 243                                       B[0], u0, umid, depthb,
 244                                       parameters );
 245             if( intersect_BB( A[0], B[1] ) )
 246                 recursively_intersect( A[0], t0, tmid, deptha,
 247                                       B[1], umid, u1, depthb,
 248                                       parameters );
 249             if( intersect_BB( A[1], B[1] ) )
 250                 recursively_intersect( A[1], tmid, t1, deptha,
 251                                       B[1], umid, u1, depthb,
 252                                       parameters );
 253         }
 254         else
 255         {
 256             if( intersect_BB( A[0], b ) )
 257                 recursively_intersect( A[0], t0, tmid, deptha,
 258                                       b, u0, u1, depthb,
 259                                       parameters );
 260             if( intersect_BB( A[1], b ) )
 261                 recursively_intersect( A[1], tmid, t1, deptha,
 262                                       b, u0, u1, depthb,
 263                                       parameters );
 264         }
 265     }
 266     else
 267         if( depthb > 0 )
 268         {
 269             OldBezier B[2];
 270             b.split(0.5, B[0], B[1]);
 271             double umid = (u0 + u1)*0.5;
 272             depthb--;
 273             if( intersect_BB( a, B[0] ) )
 274                 recursively_intersect( a, t0, t1, deptha,
 275                                       B[0], u0, umid, depthb,
 276                                       parameters );
 277             if( intersect_BB( a, B[1] ) )
 278                 recursively_intersect( a, t0, t1, deptha,
 279                                       B[0], umid, u1, depthb,
 280                                       parameters );
 281         }
 282         else // Both segments are fully subdivided; now do line segments
 283         {
 284             double xlk = a.p.back()[X] - a.p[0][X];
 285             double ylk = a.p.back()[Y] - a.p[0][Y];
 286             double xnm = b.p.back()[X] - b.p[0][X];
 287             double ynm = b.p.back()[Y] - b.p[0][Y];
 288             double xmk = b.p[0][X] - a.p[0][X];
 289             double ymk = b.p[0][Y] - a.p[0][Y];
 290             double det = xnm * ylk - ynm * xlk;
 291             if( 1.0 + det == 1.0 )
 292                 return;
 293             else
 294             {
 295                 double detinv = 1.0 / det;
 296                 double s = ( xnm * ymk - ynm *xmk ) * detinv;
 297                 double t = ( xlk * ymk - ylk * xmk ) * detinv;
 298                 if( ( s < 0.0 ) || ( s > 1.0 ) || ( t < 0.0 ) || ( t > 1.0 ) )
 299                     return;
 300                 parameters.push_back(std::pair<double, double>(t0 + s * ( t1 - t0 ),
 301                                                          u0 + t * ( u1 - u0 )));
 302             }
 303         }
 304 }
 305
 306 inline double log4( double x ) { return log(x)/log(4.); }
 307
 308 /*
 309  * Wang's theorem is used to estimate the level of subdivision required,
 310  * but only if the bounding boxes interfere at the top level.
 311  * Assuming there is a possible intersection, recursively_intersect is
 312  * used to find all the parameters corresponding to intersection points.
 313  * these are then sorted and returned in an array.
 314  */
 315
 316 double Lmax(Point p) {
 317     return std::max(fabs(p[X]), fabs(p[Y]));
 318 }
 319
 320 unsigned wangs_theorem(OldBezier a) {
 321     return 6; // seems a good approximation!
 322     double la1 = Lmax( ( a.p[2] - a.p[1] ) - (a.p[1] - a.p[0]) );
 323     double la2 = Lmax( ( a.p[3] - a.p[2] ) - (a.p[2] - a.p[1]) );
 324     double l0 = std::max(la1, la2);
 325     unsigned ra;
 326     if( l0 * 0.75 * M_SQRT2 + 1.0 == 1.0 )
 327         ra = 0;
 328     else
 329         ra = (unsigned)ceil( log4( M_SQRT2 * 6.0 / 8.0 * INV_EPS * l0 ) );
 330     //std::cout << ra << std::endl;
 331     return ra;
 332 }
 333
 334 struct rparams
 335 {
 336     OldBezier &A;
 337     OldBezier &B;
 338 };
 339
 340 static int
 341 intersect_polish_f (const gsl_vector * x, void *params,
 342               gsl_vector * f)
 343 {
 344     const double x0 = gsl_vector_get (x, 0);
 345     const double x1 = gsl_vector_get (x, 1);
 346
 347     Geom::Point dx = ((struct rparams *) params)->A(x0) -
 348         ((struct rparams *) params)->B(x1);
 349
 350     gsl_vector_set (f, 0, dx[0]);
 351     gsl_vector_set (f, 1, dx[1]);
 352
 353     return GSL_SUCCESS;
 354 }
 355
 356 union dbl_64{
 357     long long i64;
 358     double d64;
 359 };
 360
 361 static double EpsilonBy(double value, int eps)
 362 {
 363     dbl_64 s;
 364     s.d64 = value;
 365     s.i64 += eps;
 366     return s.d64;
 367 }
 368
 369
 370 static void intersect_polish_root (OldBezier &A, double &s,
 371                                    OldBezier &B, double &t) {
 372     const gsl_multiroot_fsolver_type *T;
 373     gsl_multiroot_fsolver *sol;
 374
 375     int status;
 376     size_t iter = 0;
 377
 378     const size_t n = 2;
 379     struct rparams p = {A, B};
 380     gsl_multiroot_function f = {&intersect_polish_f, n, &p};
 381
 382     double x_init[2] = {s, t};
 383     gsl_vector *x = gsl_vector_alloc (n);
 384
 385     gsl_vector_set (x, 0, x_init[0]);
 386     gsl_vector_set (x, 1, x_init[1]);
 387
 388     T = gsl_multiroot_fsolver_hybrids;
 389     sol = gsl_multiroot_fsolver_alloc (T, 2);
 390     gsl_multiroot_fsolver_set (sol, &f, x);
 391
 392     do
 393     {
 394         iter++;
 395         status = gsl_multiroot_fsolver_iterate (sol);
 396
 397         if (status)   /* check if solver is stuck */
 398             break;
 399
 400         status =
 401             gsl_multiroot_test_residual (sol->f, 1e-12);
 402     }
 403     while (status == GSL_CONTINUE && iter < 1000);
 404
 405     s = gsl_vector_get (sol->x, 0);
 406     t = gsl_vector_get (sol->x, 1);
 407
 408     gsl_multiroot_fsolver_free (sol);
 409     gsl_vector_free (x);
 410
 411     // This code does a neighbourhood search for minor improvements.
 412     double best_v = L1(A(s) - B(t));
 413     //std::cout  << "------\n" <<  best_v << std::endl;
 414     Point best(s,t);
 415     while (true) {
 416         Point trial = best;
 417         double trial_v = best_v;
 418         for(int nsi = -1; nsi < 2; nsi++) {
 419         for(int nti = -1; nti < 2; nti++) {
 420             Point n(EpsilonBy(best[0], nsi),
 421                     EpsilonBy(best[1], nti));
 422             double c = L1(A(n[0]) - B(n[1]));
 423             //std::cout << c << "; ";
 424             if (c < trial_v) {
 425                 trial = n;
 426                 trial_v = c;
 427             }
 428         }
 429         }
 430         if(trial == best) {
 431             //std::cout << "\n" << s << " -> " << s - best[0] << std::endl;
 432             //std::cout << t << " -> " << t - best[1] << std::endl;
 433             //std::cout << best_v << std::endl;
 434             s = best[0];
 435             t = best[1];
 436             return;
 437         } else {
 438             best = trial;
 439             best_v = trial_v;
 440         }
 441     }
 442 }
 443
 444
 445 void find_intersections_bezier_recursive( std::vector<std::pair<double, double> > &xs,
 446                          OldBezier a, OldBezier b)
 447 {
 448     if( intersect_BB( a, b ) )
 449     {
 450         recursively_intersect( a, 0., 1., wangs_theorem(a),
 451                                b, 0., 1., wangs_theorem(b),
 452                                xs);
 453     }
 454     /*for(unsigned i = 0; i < xs.size(); i++)
 455         intersect_polish_root(a, xs[i].first,
 456         b, xs[i].second);*/
 457     std::sort(xs.begin(), xs.end());
 458 }
 459
 460
 461 };
 462
 463 /*
 464   Local Variables:
 465   mode:c++
 466   c-file-style:"stroustrup"
 467   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 468   indent-tabs-mode:nil
 469   fill-column:99
 470   End:
 471 */
 472 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 :