src/2geom/basic-intersection.cpp

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