#include <2geom/basic-intersection.h> #include <2geom/sbasis-to-bezier.h> #include <2geom/exception.h> #ifdef HAVE_GSL #include <gsl/gsl_vector.h> #include <gsl/gsl_multiroots.h> #endif using std::vector; namespace Geom { //#ifdef USE_RECURSIVE_INTERSECTOR // void find_intersections(std::vector<std::pair<double, double> > &xs, // D2<SBasis> const & A, // D2<SBasis> const & B) { // vector<Point> BezA, BezB; // sbasis_to_bezier(BezA, A); // sbasis_to_bezier(BezB, B); // xs.clear(); // find_intersections_bezier_recursive(xs, BezA, BezB); // } // void find_intersections(std::vector< std::pair<double, double> > & xs, // std::vector<Point> const& A, // std::vector<Point> const& B, // double precision){ // find_intersections_bezier_recursive(xs, A, B, precision); // } //#else namespace detail{ namespace bezier_clipping { void portion (std::vector<Point> & B, Interval const& I); }; }; void find_intersections(std::vector<std::pair<double, double> > &xs, D2<SBasis> const & A, D2<SBasis> const & B) { vector<Point> BezA, BezB; sbasis_to_bezier(BezA, A); sbasis_to_bezier(BezB, B); xs.clear(); find_intersections_bezier_clipping(xs, BezA, BezB); } void find_intersections(std::vector< std::pair<double, double> > & xs, std::vector<Point> const& A, std::vector<Point> const& B, double precision){ find_intersections_bezier_clipping(xs, A, B, precision); } //#endif /* * split the curve at the midpoint, returning an array with the two parts * Temporary storage is minimized by using part of the storage for the result * to hold an intermediate value until it is no longer needed. */ void split(vector<Point> const &p, double t, vector<Point> &left, vector<Point> &right) { const unsigned sz = p.size(); Geom::Point Vtemp[sz][sz]; /* Copy control points */ std::copy(p.begin(), p.end(), Vtemp[0]); /* Triangle computation */ for (unsigned i = 1; i < sz; i++) { for (unsigned j = 0; j < sz - i; j++) { Vtemp[i][j] = lerp(t, Vtemp[i-1][j], Vtemp[i-1][j+1]); } } left.resize(sz); right.resize(sz); for (unsigned j = 0; j < sz; j++) left[j] = Vtemp[j][0]; for (unsigned j = 0; j < sz; j++) right[j] = Vtemp[sz-1-j][j]; } void find_self_intersections(std::vector<std::pair<double, double> > &xs, D2<SBasis> const & A) { vector<double> dr = roots(derivative(A[X])); { vector<double> dyr = roots(derivative(A[Y])); dr.insert(dr.begin(), dyr.begin(), dyr.end()); } dr.push_back(0); dr.push_back(1); // We want to be sure that we have no empty segments sort(dr.begin(), dr.end()); vector<double>::iterator new_end = unique(dr.begin(), dr.end()); dr.resize( new_end - dr.begin() ); vector<vector<Point> > pieces; { vector<Point> in, l, r; sbasis_to_bezier(in, A); for(unsigned i = 0; i < dr.size()-1; i++) { split(in, (dr[i+1]-dr[i]) / (1 - dr[i]), l, r); pieces.push_back(l); in = r; } } for(unsigned i = 0; i < dr.size()-1; i++) { for(unsigned j = i+1; j < dr.size()-1; j++) { std::vector<std::pair<double, double> > section; find_intersections( section, pieces[i], pieces[j]); for(unsigned k = 0; k < section.size(); k++) { double l = section[k].first; double r = section[k].second; // XXX: This condition will prune out false positives, but it might create some false negatives. Todo: Confirm it is correct. if(j == i+1) //if((l == 1) && (r == 0)) if( ( l > 1-1e-4 ) && (r < 1e-4) )//FIXME: what precision should be used here??? continue; xs.push_back(std::make_pair((1-l)*dr[i] + l*dr[i+1], (1-r)*dr[j] + r*dr[j+1])); } } } // Because i is in order, xs should be roughly already in order? //sort(xs.begin(), xs.end()); //unique(xs.begin(), xs.end()); } #ifdef HAVE_GSL #include <gsl/gsl_multiroots.h> struct rparams { D2<SBasis> const &A; D2<SBasis> const &B; }; static int intersect_polish_f (const gsl_vector * x, void *params, gsl_vector * f) { const double x0 = gsl_vector_get (x, 0); const double x1 = gsl_vector_get (x, 1); Geom::Point dx = ((struct rparams *) params)->A(x0) - ((struct rparams *) params)->B(x1); gsl_vector_set (f, 0, dx[0]); gsl_vector_set (f, 1, dx[1]); return GSL_SUCCESS; } #endif union dbl_64{ long long i64; double d64; }; static double EpsilonBy(double value, int eps) { dbl_64 s; s.d64 = value; s.i64 += eps; return s.d64; } 00178 static void intersect_polish_root (D2<SBasis> const &A, double &s, D2<SBasis> const &B, double &t) { #ifdef HAVE_GSL const gsl_multiroot_fsolver_type *T; gsl_multiroot_fsolver *sol; int status; size_t iter = 0; #endif std::vector<Point> as, bs; as = A.valueAndDerivatives(s, 2); bs = B.valueAndDerivatives(t, 2); Point F = as[0] - bs[0]; double best = dot(F, F); for(int i = 0; i < 4; i++) { /** we want to solve J*(x1 - x0) = f(x0) |dA(s)[0] -dB(t)[0]| (X1 - X0) = A(s) - B(t) |dA(s)[1] -dB(t)[1]| **/ // We're using the standard transformation matricies, which is numerically rather poor. Much better to solve the equation using elimination. Matrix jack(as[1][0], as[1][1], -bs[1][0], -bs[1][1], 0, 0); Point soln = (F)*jack.inverse(); double ns = s - soln[0]; double nt = t - soln[1]; as = A.valueAndDerivatives(ns, 2); bs = B.valueAndDerivatives(nt, 2); F = as[0] - bs[0]; double trial = dot(F, F); if (trial > best*0.1) {// we have standards, you know // At this point we could do a line search break; } best = trial; s = ns; t = nt; } #ifdef HAVE_GSL const size_t n = 2; struct rparams p = {A, B}; gsl_multiroot_function f = {&intersect_polish_f, n, &p}; double x_init[2] = {s, t}; gsl_vector *x = gsl_vector_alloc (n); gsl_vector_set (x, 0, x_init[0]); gsl_vector_set (x, 1, x_init[1]); T = gsl_multiroot_fsolver_hybrids; sol = gsl_multiroot_fsolver_alloc (T, 2); gsl_multiroot_fsolver_set (sol, &f, x); do { iter++; status = gsl_multiroot_fsolver_iterate (sol); if (status) /* check if solver is stuck */ break; status = gsl_multiroot_test_residual (sol->f, 1e-12); } while (status == GSL_CONTINUE && iter < 1000); s = gsl_vector_get (sol->x, 0); t = gsl_vector_get (sol->x, 1); gsl_multiroot_fsolver_free (sol); gsl_vector_free (x); #endif { // This code does a neighbourhood search for minor improvements. double best_v = L1(A(s) - B(t)); //std::cout << "------\n" << best_v << std::endl; Point best(s,t); while (true) { Point trial = best; double trial_v = best_v; for(int nsi = -1; nsi < 2; nsi++) { for(int nti = -1; nti < 2; nti++) { Point n(EpsilonBy(best[0], nsi), EpsilonBy(best[1], nti)); double c = L1(A(n[0]) - B(n[1])); //std::cout << c << "; "; if (c < trial_v) { trial = n; trial_v = c; } } } if(trial == best) { //std::cout << "\n" << s << " -> " << s - best[0] << std::endl; //std::cout << t << " -> " << t - best[1] << std::endl; //std::cout << best_v << std::endl; s = best[0]; t = best[1]; return; } else { best = trial; best_v = trial_v; } } } } void polish_intersections(std::vector<std::pair<double, double> > &xs, D2<SBasis> const &A, D2<SBasis> const &B) { for(unsigned i = 0; i < xs.size(); i++) intersect_polish_root(A, xs[i].first, B, xs[i].second); } /** * Compute the Hausdorf distance from A to B only. */ #if 0 /** Compute the value of a bezier Todo: find a good palce for this. */ // suggested by Sederberg. Point OldBezier::operator()(double t) const { int n = p.size()-1; double u, bc, tn, tmp; int i; Point r; for(int dim = 0; dim < 2; dim++) { u = 1.0 - t; bc = 1; tn = 1; tmp = p[0][dim]*u; for(i=1; i<n; i++){ tn = tn*t; bc = bc*(n-i+1)/i; tmp = (tmp + tn*bc*p[i][dim])*u; } r[dim] = (tmp + tn*t*p[n][dim]); } return r; } #endif /** * Compute the Hausdorf distance from A to B only. */ 00339 double hausdorfl(D2<SBasis>& A, D2<SBasis> const& B, double m_precision, double *a_t, double* b_t) { std::vector< std::pair<double, double> > xs; std::vector<Point> Az, Bz; sbasis_to_bezier (Az, A); sbasis_to_bezier (Bz, B); find_collinear_normal(xs, Az, Bz, m_precision); double h_dist = 0, h_a_t = 0, h_b_t = 0; double dist = 0; Point Ax = A.at0(); double t = Geom::nearest_point(Ax, B); dist = Geom::distance(Ax, B(t)); if (dist > h_dist) { h_a_t = 0; h_b_t = t; h_dist = dist; } Ax = A.at1(); t = Geom::nearest_point(Ax, B); dist = Geom::distance(Ax, B(t)); if (dist > h_dist) { h_a_t = 1; h_b_t = t; h_dist = dist; } for (size_t i = 0; i < xs.size(); ++i) { Point At = A(xs[i].first); Point Bu = B(xs[i].second); double distAtBu = Geom::distance(At, Bu); t = Geom::nearest_point(At, B); dist = Geom::distance(At, B(t)); //FIXME: we might miss it due to floating point precision... if (dist >= distAtBu-.1 && distAtBu > h_dist) { h_a_t = xs[i].first; h_b_t = xs[i].second; h_dist = distAtBu; } } if(a_t) *a_t = h_a_t; if(b_t) *b_t = h_b_t; return h_dist; } /** * Compute the symmetric Hausdorf distance. */ 00389 double hausdorf(D2<SBasis>& A, D2<SBasis> const& B, double m_precision, double *a_t, double* b_t) { double h_dist = hausdorfl(A, B, m_precision, a_t, b_t); double dist = 0; Point Bx = B.at0(); double t = Geom::nearest_point(Bx, A); dist = Geom::distance(Bx, A(t)); if (dist > h_dist) { if(a_t) *a_t = t; if(b_t) *b_t = 0; h_dist = dist; } Bx = B.at1(); t = Geom::nearest_point(Bx, A); dist = Geom::distance(Bx, A(t)); if (dist > h_dist) { if(a_t) *a_t = t; if(b_t) *b_t = 1; h_dist = dist; } return h_dist; } }; /* Local Variables: mode:c++ c-file-style:"stroustrup" c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +)) indent-tabs-mode:nil fill-column:99 End: */ // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :