1 #include <2geom/basic-intersection.h>
2 #include <2geom/sbasis-to-bezier.h>
3 #include <2geom/exception.h>
5 #ifdef HAVE_GSL
6 #include <gsl/gsl_vector.h>
7 #include <gsl/gsl_multiroots.h>
8 #endif
10 using std::vector;
11 namespace Geom {
13 //#ifdef USE_RECURSIVE_INTERSECTOR
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);
22 // xs.clear();
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 // }
33 //#else
35 namespace detail{ namespace bezier_clipping {
36 void portion (std::vector<Point> & B, Interval const& I);
37 }; };
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);
46 xs.clear();
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 }
57 //#endif
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];
69 /* Copy control points */
70 std::copy(p.begin(), p.end(), Vtemp[0]);
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 }
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 }
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() );
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 }
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;
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 }
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 }
138 #ifdef HAVE_GSL
139 #include <gsl/gsl_multiroots.h>
141 struct rparams
142 {
143 D2<SBasis> const &A;
144 D2<SBasis> const &B;
145 };
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);
154 Geom::Point dx = ((struct rparams *) params)->A(x0) -
155 ((struct rparams *) params)->B(x1);
157 gsl_vector_set (f, 0, dx[0]);
158 gsl_vector_set (f, 1, dx[1]);
160 return GSL_SUCCESS;
161 }
162 #endif
164 union dbl_64{
165 long long i64;
166 double d64;
167 };
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 }
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;
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);
193 for(int i = 0; i < 4; i++) {
195 /**
196 we want to solve
197 J*(x1 - x0) = f(x0)
199 |dA(s)[0] -dB(t)[0]| (X1 - X0) = A(s) - B(t)
200 |dA(s)[1] -dB(t)[1]|
201 **/
203 // We're using the standard transformation matricies, which is numerically rather poor. Much better to solve the equation using elimination.
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];
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 }
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};
230 double x_init[2] = {s, t};
231 gsl_vector *x = gsl_vector_alloc (n);
233 gsl_vector_set (x, 0, x_init[0]);
234 gsl_vector_set (x, 1, x_init[1]);
236 T = gsl_multiroot_fsolver_hybrids;
237 sol = gsl_multiroot_fsolver_alloc (T, 2);
238 gsl_multiroot_fsolver_set (sol, &f, x);
240 do
241 {
242 iter++;
243 status = gsl_multiroot_fsolver_iterate (sol);
245 if (status) /* check if solver is stuck */
246 break;
248 status =
249 gsl_multiroot_test_residual (sol->f, 1e-12);
250 }
251 while (status == GSL_CONTINUE && iter < 1000);
253 s = gsl_vector_get (sol->x, 0);
254 t = gsl_vector_get (sol->x, 1);
256 gsl_multiroot_fsolver_free (sol);
257 gsl_vector_free (x);
258 #endif
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 }
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 }
305 /**
306 * Compute the Hausdorf distance from A to B only.
307 */
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
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 }
379 }
380 if(a_t) *a_t = h_a_t;
381 if(b_t) *b_t = h_b_t;
383 return h_dist;
384 }
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);
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 }
412 return h_dist;
413 }
414 };
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:fileencoding=utf-8:textwidth=99 :