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