src/2geom/basic-intersection.cpp

   1 #include <2geom/basic-intersection.h>
   2 #include <2geom/sbasis-to-bezier.h>
   3 #include <2geom/exception.h>
   4
   5 #ifdef HAVE_GSL
   6 #include <gsl/gsl_vector.h>
   7 #include <gsl/gsl_multiroots.h>
   8 #endif
   9
  10 using std::vector;
  11 namespace Geom {
  12
  13 //#ifdef USE_RECURSIVE_INTERSECTOR
  14
  15 // void find_intersections(std::vector<std::pair<double, double> > &xs,
  16 //                         D2<SBasis> const & A,
  17 //                         D2<SBasis> const & B) {
  18 //     vector<Point> BezA, BezB;
  19 //     sbasis_to_bezier(BezA, A);
  20 //     sbasis_to_bezier(BezB, B);
  21
  22 //     xs.clear();
  23
  24 //     find_intersections_bezier_recursive(xs, BezA, BezB);
  25 // }
  26 // void find_intersections(std::vector< std::pair<double, double> > & xs,
  27 //                          std::vector<Point> const& A,
  28 //                          std::vector<Point> const& B,
  29 //                         double precision){
  30 //     find_intersections_bezier_recursive(xs, A, B, precision);
  31 // }
  32
  33 //#else
  34
  35 namespace detail{ namespace bezier_clipping {
  36 void portion (std::vector<Point> & B, Interval const& I);
  37 }; };
  38
  39 void find_intersections(std::vector<std::pair<double, double> > &xs,
  40                         D2<SBasis> const & A,
  41                         D2<SBasis> const & B) {
  42     vector<Point> BezA, BezB;
  43     sbasis_to_bezier(BezA, A);
  44     sbasis_to_bezier(BezB, B);
  45
  46     xs.clear();
  47
  48     find_intersections_bezier_clipping(xs, BezA, BezB);
  49 }
  50 void find_intersections(std::vector< std::pair<double, double> > & xs,
  51                          std::vector<Point> const& A,
  52                          std::vector<Point> const& B,
  53                         double precision){
  54     find_intersections_bezier_clipping(xs, A, B, precision);
  55 }
  56
  57 //#endif
  58
  59 /*
  60  * split the curve at the midpoint, returning an array with the two parts
  61  * Temporary storage is minimized by using part of the storage for the result
  62  * to hold an intermediate value until it is no longer needed.
  63  */
  64 void split(vector<Point> const &p, double t,
  65            vector<Point> &left, vector<Point> &right) {
  66     const unsigned sz = p.size();
  67     Geom::Point Vtemp[sz][sz];
  68
  69     /* Copy control points      */
  70     std::copy(p.begin(), p.end(), Vtemp[0]);
  71
  72     /* Triangle computation     */
  73     for (unsigned i = 1; i < sz; i++) {
  74         for (unsigned j = 0; j < sz - i; j++) {
  75             Vtemp[i][j] = lerp(t, Vtemp[i-1][j], Vtemp[i-1][j+1]);
  76         }
  77     }
  78
  79     left.resize(sz);
  80     right.resize(sz);
  81     for (unsigned j = 0; j < sz; j++)
  82         left[j]  = Vtemp[j][0];
  83     for (unsigned j = 0; j < sz; j++)
  84         right[j] = Vtemp[sz-1-j][j];
  85 }
  86
  87
  88 void
  89 find_self_intersections(std::vector<std::pair<double, double> > &xs,
  90                         D2<SBasis> const & A) {
  91     vector<double> dr = roots(derivative(A[X]));
  92     {
  93         vector<double> dyr = roots(derivative(A[Y]));
  94         dr.insert(dr.begin(), dyr.begin(), dyr.end());
  95     }
  96     dr.push_back(0);
  97     dr.push_back(1);
  98     // We want to be sure that we have no empty segments
  99     sort(dr.begin(), dr.end());
 100     vector<double>::iterator new_end = unique(dr.begin(), dr.end());
 101     dr.resize( new_end - dr.begin() );
 102
 103     vector<vector<Point> > pieces;
 104     {
 105         vector<Point> in, l, r;
 106         sbasis_to_bezier(in, A);
 107         for(unsigned i = 0; i < dr.size()-1; i++) {
 108             split(in, (dr[i+1]-dr[i]) / (1 - dr[i]), l, r);
 109             pieces.push_back(l);
 110             in = r;
 111         }
 112     }
 113
 114     for(unsigned i = 0; i < dr.size()-1; i++) {
 115         for(unsigned j = i+1; j < dr.size()-1; j++) {
 116             std::vector<std::pair<double, double> > section;
 117
 118             find_intersections( section, pieces[i], pieces[j]);
 119             for(unsigned k = 0; k < section.size(); k++) {
 120                 double l = section[k].first;
 121                 double r = section[k].second;
 122 // XXX: This condition will prune out false positives, but it might create some false negatives.  Todo: Confirm it is correct.
 123                 if(j == i+1)
 124                     //if((l == 1) && (r == 0))
 125                     if( ( l > 1-1e-4 ) && (r < 1e-4) )//FIXME: what precision should be used here???
 126                         continue;
 127                 xs.push_back(std::make_pair((1-l)*dr[i] + l*dr[i+1],
 128                                                 (1-r)*dr[j] + r*dr[j+1]));
 129             }
 130         }
 131     }
 132
 133     // Because i is in order, xs should be roughly already in order?
 134     //sort(xs.begin(), xs.end());
 135     //unique(xs.begin(), xs.end());
 136 }
 137
 138 #ifdef HAVE_GSL
 139 #include <gsl/gsl_multiroots.h>
 140
 141 struct rparams
 142 {
 143     D2<SBasis> const &A;
 144     D2<SBasis> const &B;
 145 };
 146
 147 static int
 148 intersect_polish_f (const gsl_vector * x, void *params,
 149                     gsl_vector * f)
 150 {
 151     const double x0 = gsl_vector_get (x, 0);
 152     const double x1 = gsl_vector_get (x, 1);
 153
 154     Geom::Point dx = ((struct rparams *) params)->A(x0) -
 155         ((struct rparams *) params)->B(x1);
 156
 157     gsl_vector_set (f, 0, dx[0]);
 158     gsl_vector_set (f, 1, dx[1]);
 159
 160     return GSL_SUCCESS;
 161 }
 162 #endif
 163
 164 union dbl_64{
 165     long long i64;
 166     double d64;
 167 };
 168
 169 static double EpsilonBy(double value, int eps)
 170 {
 171     dbl_64 s;
 172     s.d64 = value;
 173     s.i64 += eps;
 174     return s.d64;
 175 }
 176
 177
 178 static void intersect_polish_root (D2<SBasis> const &A, double &s,
 179                                    D2<SBasis> const &B, double &t) {
 180 #ifdef HAVE_GSL
 181     const gsl_multiroot_fsolver_type *T;
 182     gsl_multiroot_fsolver *sol;
 183
 184     int status;
 185     size_t iter = 0;
 186 #endif
 187     std::vector<Point> as, bs;
 188     as = A.valueAndDerivatives(s, 2);
 189     bs = B.valueAndDerivatives(t, 2);
 190     Point F = as[0] - bs[0];
 191     double best = dot(F, F);
 192
 193     for(int i = 0; i < 4; i++) {
 194
 195         /**
 196            we want to solve
 197            J*(x1 - x0) = f(x0)
 198
 199            |dA(s)[0]  -dB(t)[0]|  (X1 - X0) = A(s) - B(t)
 200            |dA(s)[1]  -dB(t)[1]|
 201         **/
 202
 203         // We're using the standard transformation matricies, which is numerically rather poor.  Much better to solve the equation using elimination.
 204
 205         Matrix jack(as[1][0], as[1][1],
 206                     -bs[1][0], -bs[1][1],
 207                     0, 0);
 208         Point soln = (F)*jack.inverse();
 209         double ns = s - soln[0];
 210         double nt = t - soln[1];
 211
 212         as = A.valueAndDerivatives(ns, 2);
 213         bs = B.valueAndDerivatives(nt, 2);
 214         F = as[0] - bs[0];
 215         double trial = dot(F, F);
 216         if (trial > best*0.1) {// we have standards, you know
 217             // At this point we could do a line search
 218             break;
 219         }
 220         best = trial;
 221         s = ns;
 222         t = nt;
 223     }
 224
 225 #ifdef HAVE_GSL
 226     const size_t n = 2;
 227     struct rparams p = {A, B};
 228     gsl_multiroot_function f = {&intersect_polish_f, n, &p};
 229
 230     double x_init[2] = {s, t};
 231     gsl_vector *x = gsl_vector_alloc (n);
 232
 233     gsl_vector_set (x, 0, x_init[0]);
 234     gsl_vector_set (x, 1, x_init[1]);
 235
 236     T = gsl_multiroot_fsolver_hybrids;
 237     sol = gsl_multiroot_fsolver_alloc (T, 2);
 238     gsl_multiroot_fsolver_set (sol, &f, x);
 239
 240     do
 241     {
 242         iter++;
 243         status = gsl_multiroot_fsolver_iterate (sol);
 244
 245         if (status)   /* check if solver is stuck */
 246             break;
 247
 248         status =
 249             gsl_multiroot_test_residual (sol->f, 1e-12);
 250     }
 251     while (status == GSL_CONTINUE && iter < 1000);
 252
 253     s = gsl_vector_get (sol->x, 0);
 254     t = gsl_vector_get (sol->x, 1);
 255
 256     gsl_multiroot_fsolver_free (sol);
 257     gsl_vector_free (x);
 258 #endif
 259
 260     {
 261     // This code does a neighbourhood search for minor improvements.
 262     double best_v = L1(A(s) - B(t));
 263     //std::cout  << "------\n" <<  best_v << std::endl;
 264     Point best(s,t);
 265     while (true) {
 266         Point trial = best;
 267         double trial_v = best_v;
 268         for(int nsi = -1; nsi < 2; nsi++) {
 269         for(int nti = -1; nti < 2; nti++) {
 270             Point n(EpsilonBy(best[0], nsi),
 271                     EpsilonBy(best[1], nti));
 272             double c = L1(A(n[0]) - B(n[1]));
 273             //std::cout << c << "; ";
 274             if (c < trial_v) {
 275                 trial = n;
 276                 trial_v = c;
 277             }
 278         }
 279         }
 280         if(trial == best) {
 281             //std::cout << "\n" << s << " -> " << s - best[0] << std::endl;
 282             //std::cout << t << " -> " << t - best[1] << std::endl;
 283             //std::cout << best_v << std::endl;
 284             s = best[0];
 285             t = best[1];
 286             return;
 287         } else {
 288             best = trial;
 289             best_v = trial_v;
 290         }
 291     }
 292     }
 293 }
 294
 295
 296 void polish_intersections(std::vector<std::pair<double, double> > &xs,
 297                         D2<SBasis> const  &A, D2<SBasis> const &B)
 298 {
 299     for(unsigned i = 0; i < xs.size(); i++)
 300         intersect_polish_root(A, xs[i].first,
 301                               B, xs[i].second);
 302 }
 303
 304
 305  /**
 306   * Compute the Hausdorf distance from A to B only.
 307   */
 308
 309
 310 #if 0
 311 /** Compute the value of a bezier
 312     Todo: find a good palce for this.
 313  */
 314 // suggested by Sederberg.
 315 Point OldBezier::operator()(double t) const {
 316     int n = p.size()-1;
 317     double u, bc, tn, tmp;
 318     int i;
 319     Point r;
 320     for(int dim = 0; dim < 2; dim++) {
 321         u = 1.0 - t;
 322         bc = 1;
 323         tn = 1;
 324         tmp = p[0][dim]*u;
 325         for(i=1; i<n; i++){
 326             tn = tn*t;
 327             bc = bc*(n-i+1)/i;
 328             tmp = (tmp + tn*bc*p[i][dim])*u;
 329         }
 330         r[dim] = (tmp + tn*t*p[n][dim]);
 331     }
 332     return r;
 333 }
 334 #endif
 335
 336 /**
 337  * Compute the Hausdorf distance from A to B only.
 338  */
 339 double hausdorfl(D2<SBasis>& A, D2<SBasis> const& B,
 340                  double m_precision,
 341                  double *a_t, double* b_t) {
 342     std::vector< std::pair<double, double> > xs;
 343     std::vector<Point> Az, Bz;
 344     sbasis_to_bezier (Az, A);
 345     sbasis_to_bezier (Bz, B);
 346     find_collinear_normal(xs, Az, Bz, m_precision);
 347     double h_dist = 0, h_a_t = 0, h_b_t = 0;
 348     double dist = 0;
 349     Point Ax = A.at0();
 350     double t = Geom::nearest_point(Ax, B);
 351     dist = Geom::distance(Ax, B(t));
 352     if (dist > h_dist) {
 353         h_a_t = 0;
 354         h_b_t = t;
 355         h_dist = dist;
 356     }
 357     Ax = A.at1();
 358     t = Geom::nearest_point(Ax, B);
 359     dist = Geom::distance(Ax, B(t));
 360     if (dist > h_dist) {
 361         h_a_t = 1;
 362         h_b_t = t;
 363         h_dist = dist;
 364     }
 365     for (size_t i = 0; i < xs.size(); ++i)
 366     {
 367         Point At = A(xs[i].first);
 368         Point Bu = B(xs[i].second);
 369         double distAtBu = Geom::distance(At, Bu);
 370         t = Geom::nearest_point(At, B);
 371         dist = Geom::distance(At, B(t));
 372         //FIXME: we might miss it due to floating point precision...
 373         if (dist >= distAtBu-.1 && distAtBu > h_dist) {
 374             h_a_t = xs[i].first;
 375             h_b_t = xs[i].second;
 376             h_dist = distAtBu;
 377         }
 378
 379     }
 380     if(a_t) *a_t = h_a_t;
 381     if(b_t) *b_t = h_b_t;
 382
 383     return h_dist;
 384 }
 385
 386 /**
 387  * Compute the symmetric Hausdorf distance.
 388  */
 389 double hausdorf(D2<SBasis>& A, D2<SBasis> const& B,
 390                  double m_precision,
 391                  double *a_t, double* b_t) {
 392     double h_dist = hausdorfl(A, B, m_precision, a_t, b_t);
 393
 394     double dist = 0;
 395     Point Bx = B.at0();
 396     double t = Geom::nearest_point(Bx, A);
 397     dist = Geom::distance(Bx, A(t));
 398     if (dist > h_dist) {
 399         if(a_t) *a_t = t;
 400         if(b_t) *b_t = 0;
 401         h_dist = dist;
 402     }
 403     Bx = B.at1();
 404     t = Geom::nearest_point(Bx, A);
 405     dist = Geom::distance(Bx, A(t));
 406     if (dist > h_dist) {
 407         if(a_t) *a_t = t;
 408         if(b_t) *b_t = 1;
 409         h_dist = dist;
 410     }
 411
 412     return h_dist;
 413 }
 414 };
 415
 416 /*
 417   Local Variables:
 418   mode:c++
 419   c-file-style:"stroustrup"
 420   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 421   indent-tabs-mode:nil
 422   fill-column:99
 423   End:
 424 */
 425 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :