1 /*
2 * Nearest Points Toy 3
3 *
4 * Authors:
5 * Nathan Hurst <njh at njhurst.com>
6 * Marco Cecchetti <mrcekets at gmail.com>
7 *
8 * Copyright 2008 authors
9 *
10 * This library is free software; you can redistribute it and/or
11 * modify it either under the terms of the GNU Lesser General Public
12 * License version 2.1 as published by the Free Software Foundation
13 * (the "LGPL") or, at your option, under the terms of the Mozilla
14 * Public License Version 1.1 (the "MPL"). If you do not alter this
15 * notice, a recipient may use your version of this file under either
16 * the MPL or the LGPL.
17 *
18 * You should have received a copy of the LGPL along with this library
19 * in the file COPYING-LGPL-2.1; if not, write to the Free Software
20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21 * You should have received a copy of the MPL along with this library
22 * in the file COPYING-MPL-1.1
23 *
24 * The contents of this file are subject to the Mozilla Public License
25 * Version 1.1 (the "License"); you may not use this file except in
26 * compliance with the License. You may obtain a copy of the License at
27 * http://www.mozilla.org/MPL/
28 *
29 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
30 * OF ANY KIND, either express or implied. See the LGPL or the MPL for
31 * the specific language governing rights and limitations.
32 */
33
34
35 #include <2geom/d2.h>
36 #include <2geom/sbasis.h>
37 #include <2geom/path.h>
38 #include <2geom/bezier-to-sbasis.h>
39 #include <2geom/sbasis-geometric.h>
40 #include <2geom/piecewise.h>
41 #include <2geom/path-intersection.h>
42
43 #include <toys/path-cairo.h>
44 #include <toys/toy-framework-2.h>
45
46 #include <algorithm>
47
48
49 using namespace Geom;
50
51
52 class np_finder
53 {
54 public:
np_finder(cairo_t * _cr,D2<SBasis> const & _c1,D2<SBasis> const & _c2)55 np_finder(cairo_t* _cr, D2<SBasis> const& _c1, D2<SBasis> const& _c2)
56 : cr(_cr), cc1(_c1), cc2(_c2), c1(_c1), c2(_c2)
57 {
58
59 dc1 = derivative(_c1);
60 dc2 = derivative(_c2);
61 cd1 = dot(_c1,dc1);
62 cd2 = dot(_c2,dc2);
63 dsq = 10e30;
64
65 Piecewise< D2<SBasis> > uv1 = unitVector(dc1, EPSILON);
66 Piecewise< D2<SBasis> > uv2 = unitVector(dc2, EPSILON);
67
68 dcn1 = dot(Piecewise< D2<SBasis> >(dc1), uv1);
69 dcn2 = dot(Piecewise< D2<SBasis> >(dc2), uv2);
70
71 r_dcn1 = cross(derivative(uv1), uv1);
72 r_dcn2 = cross(derivative(uv2), uv2);
73
74 k1 = Geom::divide(r_dcn1, dcn1, EPSILON, 3);
75 k2 = Geom::divide(r_dcn2, dcn2, EPSILON, 3);
76
77
78 n1 = divide(rot90(uv1), k1, EPSILON, 3);
79 n2 = divide(rot90(uv2), k2, EPSILON, 3);
80
81 std::vector<double> cuts1, cuts2;
82
83 // add cuts at points where the curvature is discontinuos
84 for ( unsigned int i = 1; i < k1.size(); ++i )
85 {
86 if( !are_near(k1[i-1].at1(), k1[i].at0()) )
87 {
88 cuts1.push_back(k1.cuts[i]);
89 }
90 }
91 for ( unsigned int i = 1; i < k2.size(); ++i )
92 {
93 if( !are_near(k2[i-1].at1(), k2[i].at0()) )
94 {
95 cuts2.push_back(k2.cuts[i]);
96 }
97 }
98
99 c1 = partition(c1, cuts1);
100 c2 = partition(c2, cuts2);
101
102 // std::cerr << "# k1 discontinuitis" << std::endl;
103 // for( unsigned int i = 0; i < cuts1.size(); ++i )
104 // {
105 // std::cerr << "[" << i << "]= " << cuts1[i] << std::endl;
106 // }
107 // std::cerr << "# k2 discontinuitis" << std::endl;
108 // for( unsigned int i = 0; i < cuts2.size(); ++i )
109 // {
110 // std::cerr << "[" << i << "]= " << cuts2[i] << std::endl;
111 // }
112
113 // add cuts at points were the curvature is zero
114 std::vector<double> k1_roots = roots(k1);
115 std::vector<double> k2_roots = roots(k2);
116 std::sort(k1_roots.begin(), k1_roots.end());
117 std::sort(k2_roots.begin(), k2_roots.end());
118 c1 = partition(c1, k1_roots);
119 c2 = partition(c2, k2_roots);
120
121 // std::cerr << "# k1 zeros" << std::endl;
122 // for( unsigned int i = 0; i < k1_roots.size(); ++i )
123 // {
124 // std::cerr << "[" << i << "]= " << k1_roots[i] << std::endl;
125 // }
126 // std::cerr << "# k2 zeros" << std::endl;
127 // for( unsigned int i = 0; i < k2_roots.size(); ++i )
128 // {
129 // std::cerr << "[" << i << "]= " << k2_roots[i] << std::endl;
130 // }
131
132
133 cairo_set_line_width(cr, 0.2);
134 // cairo_set_source_rgba(cr, 0.0, 0.5, 0.0, 1.0);
135 // for( unsigned int i = 1; i < c1.size(); ++i )
136 // {
137 // draw_circ(cr, c1[i].at0() );
138 // }
139 // for( unsigned int i = 1; i < c2.size(); ++i )
140 // {
141 // draw_circ(cr, c2[i].at0() );
142 // }
143
144
145 // add cuts at nearest points to the other curve cuts points
146 cuts1.clear();
147 cuts1.reserve(c1.size()+1);
148 for ( unsigned int i = 0; i < c1.size(); ++i )
149 {
150 cuts1.push_back( nearest_time(c1[i].at0(), _c2, dc2, cd2) );
151 }
152 cuts1.push_back( nearest_time(c1[c1.size()-1].at1(), _c2, dc2, cd2) );
153
154 // for ( unsigned int i = 0; i < c1.size(); ++i )
155 // {
156 // cairo_move_to( cr, c1[i].at0() );
157 // cairo_line_to(cr, c2(cuts1[i]) );
158 // }
159 // cairo_move_to( cr, c1[c1.size()-1].at1() );
160 // cairo_line_to(cr, c2(cuts1[c1.size()]));
161
162 std::sort(cuts1.begin(), cuts1.end());
163
164 cuts2.clear();
165 cuts2.reserve(c2.size()+1);
166 for ( unsigned int i = 0; i < c2.size(); ++i )
167 {
168 cuts2.push_back( nearest_time(c2[i].at0(), _c1, dc1, cd1) );
169 }
170 cuts2.push_back( nearest_time(c2[c2.size()-1].at1(), _c1, dc1, cd1) );
171
172 // for ( unsigned int i = 0; i < c2.size(); ++i )
173 // {
174 // cairo_move_to( cr, c2[i].at0() );
175 // cairo_line_to(cr, c1(cuts2[i]) );
176 // }
177 // cairo_move_to( cr, c2[c2.size()-1].at1() );
178 // cairo_line_to(cr, c1(cuts2[c2.size()]));
179 // cairo_stroke(cr);
180
181 std::sort(cuts2.begin(), cuts2.end());
182
183 c1 = partition(c1, cuts2);
184 c2 = partition(c2, cuts1);
185
186
187 // copy curve to preserve cuts status
188 Piecewise< D2<SBasis> > pwc1 = c1;
189 n1 = partition(n1, pwc1.cuts);
190 pwc1 = partition(pwc1, n1.cuts);
191 r_dcn1 = partition(r_dcn1, n1.cuts);
192 Piecewise< D2<SBasis> > pwc2 = c2;
193 n2 = partition(n2, pwc2.cuts);
194 pwc2 = partition(pwc2, n2.cuts);
195
196 assert( pwc1.size() == n1.size() );
197 assert( pwc2.size() == n2.size() );
198 assert( r_dcn1.size() == n1.size() );
199
200 // add cuts at curvature max and min points
201 Piecewise<SBasis> dk1 = derivative(k1);
202 Piecewise<SBasis> dk2 = derivative(k2);
203 std::vector<double> dk1_roots = roots(dk1);
204 std::vector<double> dk2_roots = roots(dk2);
205 std::sort(dk1_roots.begin(), dk1_roots.end());
206 std::sort(dk2_roots.begin(), dk2_roots.end());
207
208 c1 = partition(c1, dk1_roots);
209 c2 = partition(c2, dk2_roots);
210
211 // std::cerr << "# k1 min/max" << std::endl;
212 // for( unsigned int i = 0; i < dk1_roots.size(); ++i )
213 // {
214 // std::cerr << "[" << i << "]= " << dk1_roots[i] << std::endl;
215 // }
216 // std::cerr << "# k2 min/max" << std::endl;
217 // for( unsigned int i = 0; i < dk2_roots.size(); ++i )
218 // {
219 // std::cerr << "[" << i << "]= " << dk2_roots[i] << std::endl;
220 // }
221
222 // cairo_set_source_rgba(cr, 0.0, 0.0, 0.6, 1.0);
223 // for( unsigned int i = 0; i < dk1_roots.size(); ++i )
224 // {
225 // draw_handle(cr, c1(dk1_roots[i]));
226 // }
227 // for( unsigned int i = 0; i < dk2_roots.size(); ++i )
228 // {
229 // draw_handle(cr, c2(dk2_roots[i]));
230 // }
231
232
233 // add cuts at nearest points to max and min curvature points
234 // of the other curve
235 cuts1.clear();
236 cuts1.reserve(dk2_roots.size());
237 for ( unsigned int i = 0; i < dk2_roots.size(); ++i )
238 {
239 cuts1.push_back(nearest_time(_c2(dk2_roots[i]), _c1, dc1, cd1));
240 }
241
242 // for( unsigned int i = 0; i < dk2_roots.size(); ++i )
243 // {
244 // cairo_move_to(cr, c2(dk2_roots[i]));
245 // cairo_line_to(cr, c1(cuts1[i]));
246 // }
247 // cairo_stroke(cr);
248
249 std::sort(cuts1.begin(), cuts1.end());
250 c1 = partition(c1, cuts1);
251
252
253 // swap normal vector direction and fill the skip list
254 skip_list.clear();
255 skip_list.resize(c1.size(), false);
256 double npt;
257 Point p, nv;
258 unsigned int si;
259 for ( unsigned int i = 0; i < pwc1.size(); ++i )
260 {
261 p = pwc1[i](0.5);
262 nv = n1[i](0.5);
263 npt = nearest_time(p, _c2, dc2, cd2);
264 if( dot( _c2(npt) - p, nv ) > 0 )
265 {
266 if ( dot( nv, n2(npt) ) > 0 )
267 {
268 n1[i] = -n1[i];
269 r_dcn1[i] = -r_dcn1[i];
270 }
271 else
272 {
273 si = c1.segN( n1.mapToDomain(0.5, i) );
274 skip_list[si] = true;
275 }
276 }
277 }
278
279
280 for ( unsigned int i = 0; i < pwc2.size(); ++i )
281 {
282 p = pwc2[i](0.5);
283 nv = n2[i](0.5);
284 npt = nearest_time(p, _c1, dc1, cd1);
285 if( dot( _c1(npt) - p, nv ) > 0 )
286 {
287 if ( dot( nv, n1(npt) ) > 0 )
288 {
289 n2[i] = -n2[i];
290 }
291 }
292 }
293
294
295 evl1 = c1 + n1;
296 evl2 = c2 + n2;
297
298 // cairo_set_source_rgba(cr, 0.3, 0.3, 0.3, 1.0);
299 // for ( unsigned int i = 0; i < c1.size(); ++i )
300 // {
301 // double t = c1.mapToDomain(0.5, i);
302 // cairo_move_to(cr, c1(t));
303 // cairo_line_to(cr, c1(t) + 30*unit_vector(n1(t)));
304 // }
305 //
306 // for ( unsigned int i = 0; i < c2.size(); ++i )
307 // {
308 // double t = c2.mapToDomain(0.5, i);
309 // cairo_move_to(cr, c2(t));
310 // cairo_line_to(cr, c2(t) + 30*unit_vector(n2(t)));
311 // }
312 // cairo_stroke(cr);
313
314 std::cerr << "# skip list: ";
315 for( unsigned int i = 0; i < c1.cuts.size(); ++i )
316 {
317 if ( skip_list[i] )
318 std::cerr << i << " ";
319 }
320 std::cerr << std::endl;
321
322 cairo_set_line_width(cr, 0.4);
323 cairo_set_source_rgba(cr, 0.6, 0.0, 0.0, 1.0);
324 for( unsigned int i = 0; i < c1.size(); ++i )
325 {
326 if ( skip_list[i] )
327 {
328 cairo_move_to(cr, c1[i].at0());
329 cairo_line_to(cr, c1[i].at1());
330 }
331 }
332 cairo_stroke(cr);
333
334 cairo_set_source_rgba(cr, 0.2, 0.2, 0.2, 1.0);
335 for( unsigned int i = 1; i < c1.size(); ++i )
336 {
337 draw_circ(cr, c1[i].at0() );
338 }
339 cairo_stroke(cr);
340
341 std::cerr << "# c1 cuts: " << std::endl;
342 for( unsigned int i = 0; i < c1.cuts.size(); ++i )
343 {
344 std::cerr << "c1.cuts[" << i << "]= " << c1.cuts[i] << std::endl;
345 }
346
347 }
348
operator ()()349 void operator() ()
350 {
351 nearest_times_impl();
352 d = sqrt(dsq);
353 }
354
firstPoint() const355 Point firstPoint() const
356 {
357 return p1;
358 }
359
secondPoint() const360 Point secondPoint() const
361 {
362 return p2;
363 }
364
firstValue() const365 double firstValue() const
366 {
367 return t1;
368 }
369
secondValue() const370 double secondValue() const
371 {
372 return t2;
373 }
374
distance() const375 double distance() const
376 {
377 return d;
378 }
379
380 private:
nearest_times_impl()381 void nearest_times_impl()
382 {
383 double t;
384 for ( unsigned int i = 0; i < c1.size(); ++i )
385 {
386 if ( skip_list[i] ) continue;
387 std::cerr << i << " ";
388 t = c1.mapToDomain(0.5, i);
389 std::pair<double, double> npc = loc_nearest_times(t, c1.cuts[i], c1.cuts[i+1]);
390 if ( npc.second != -1 && dsq > L2sq(c1(npc.first) - c2(npc.second)) )
391 {
392 t1 = npc.first;
393 t2 = npc.second;
394 p1 = c1(t1);
395 p2 = c2(t2);
396 dsq = L2sq(p1 - p2);
397 }
398 }
399 }
400
401 std::pair<double, double>
loc_nearest_times(double t,double from=0,double to=1)402 loc_nearest_times( double t, double from = 0, double to = 1 )
403 {
404 std::cerr << "[" << from << "," << to << "] t: " << t << std::endl;
405 unsigned int iter = 0, iter1 = 0, iter2 = 0;
406 std::pair<double, double> np(-1,-1);
407 std::pair<double, double> npf(from, -1);
408 std::pair<double, double> npt(to, -1);
409 double ct = t;
410 double pt = -1;
411 double s = nearest_time(c1(t), cc2, dc2, cd2);
412 cairo_set_source_rgba(cr, 1/(t+1), t*t, t, 1.0);
413 cairo_move_to(cr, c1(t));
414 while( !are_near(ct, pt) && iter < 1000 )
415 {
416 pt = ct;
417 double angle = angle_between( n1(ct), evl2(s) - evl1(ct) );
418 assert( !std::isnan(angle) );
419 angle = (angle > 0) ? angle - M_PI : angle + M_PI;
420 if ( std::fabs(angle) < M_PI/12 )
421 {
422 ++iter2;
423 // cairo_move_to(cr, c1(ct));
424 // cairo_line_to(cr, evl1(ct));
425 // cairo_line_to(cr, evl2(s));
426 //std::cerr << "s: " << s << std::endl;
427 //std::cerr << "t: " << ct << std::endl;
428
429 ct = ct + angle / r_dcn1(ct);
430 s = nearest_time(c1(ct), cc2, dc2, cd2);
431 // angle = angle_between( n2(s), evl1(ct) - evl2(s) );
432 // assert( !std::isnan(angle) );
433 // angle = (angle > 0) ? angle - M_PI : angle + M_PI;
434 // s = s + angle / (dcn2(s) * k2(s));
435 }
436 else
437 {
438 ++iter1;
439 ct = nearest_time(c2(s), cc1, dc1, cd1, from, to);
440 s = nearest_time(c1(ct), cc2, dc2, cd2);
441 }
442 iter = iter1 + iter2;
443 //std::cerr << "s: " << s << std::endl;
444 //std::cerr << "t: " << ct << std::endl;
445 //cairo_line_to(cr, c2(s));
446 //cairo_line_to(cr, c1(ct));
447 //std::cerr << "d(pt, ct) = " << std::fabs(ct - pt) << std::endl;
448 if ( ct < from )
449 {
450 std::cerr << "break left" << std::endl;
451 np = npf;
452 break;
453 }
454 if ( ct > to )
455 {
456 std::cerr << "break right" << std::endl;
457 np =npt;
458 break;
459 }
460 }
461 //std::cerr << "\n \n";
462 std::cerr << "iterations: " << iter1 << " + " << iter2 << " = "<< iter << std::endl;
463 assert(iter < 3000);
464 //cairo_move_to(cr, c1(ct));
465 //cairo_line_to(cr, c2(s));
466 cairo_stroke(cr);
467 np.first = ct;
468 np.second = s;
469 return np;
470 }
471
nearest_time(Point const & p,D2<SBasis> const & c,D2<SBasis> const & dc,SBasis const & cd,double from=0,double to=1)472 double nearest_time( Point const& p, D2<SBasis> const&c, D2<SBasis> const& dc, SBasis const& cd, double from = 0, double to = 1 )
473 {
474 D2<SBasis> sbc = c - p;
475 SBasis dd = cd - dotp(p, dc);
476 std::vector<double> zeros = roots(dd);
477 double closest = from;
478 double distsq = L2sq(sbc(from));
479 for ( unsigned int i = 0; i < zeros.size(); ++i )
480 {
481 if ( distsq > L2sq(sbc(zeros[i])) )
482 {
483 closest = zeros[i];
484 distsq = L2sq(sbc(closest));
485 }
486 }
487 if ( distsq > L2sq(sbc(to)) )
488 closest = to;
489 return closest;
490 }
491
dotp(Point const & p,D2<SBasis> const & c)492 SBasis dotp(Point const& p, D2<SBasis> const& c)
493 {
494 SBasis d;
495 d.resize(c[X].size());
496 for ( unsigned int i = 0; i < c[0].size(); ++i )
497 {
498 for( unsigned int j = 0; j < 2; ++j )
499 d[i][j] = p[X] * c[X][i][j] + p[Y] * c[Y][i][j];
500 }
501 return d;
502 }
503
504 Piecewise< D2<SBasis> >
divide(Piecewise<D2<SBasis>> const & a,Piecewise<SBasis> const & b,double tol,unsigned int k,double zero=1.e-3)505 divide( Piecewise< D2<SBasis> > const& a, Piecewise<SBasis> const& b, double tol, unsigned int k, double zero=1.e-3)
506 {
507 D2< Piecewise<SBasis> > aa = make_cuts_independent(a);
508 D2< Piecewise<SBasis> > q(Geom::divide(aa[0], b, tol, k, zero), Geom::divide(aa[1], b, tol, k, zero));
509 return sectionize(q);
510 }
511
512 struct are_near_
513 {
operator ()np_finder::are_near_514 bool operator() (double x, double y, double eps = Geom::EPSILON )
515 {
516 return are_near(x, y, eps);
517 }
518 };
519
520 private:
521 cairo_t* cr;
522 D2<SBasis> const& cc1, cc2;
523 Piecewise< D2<SBasis> > c1, c2;
524 D2<SBasis> dc1, dc2;
525 SBasis cd1, cd2;
526 Piecewise< D2<SBasis> > n1, n2, evl1, evl2;
527 Piecewise<SBasis> k1, k2, dcn1, dcn2, r_dcn1, r_dcn2;
528 double t1, t2, d, dsq;
529 Point p1, p2;
530 std::vector<bool> skip_list;
531 };
532
533
534
535
536 class NearestPoints : public Toy
537 {
538 private:
draw(cairo_t * cr,std::ostringstream * notify,int width,int height,bool save,std::ostringstream * timer_stream)539 void draw( cairo_t *cr, std::ostringstream *notify,
540 int width, int height, bool save, std::ostringstream *timer_stream)
541 {
542 cairo_set_line_width (cr, 0.3);
543 D2<SBasis> A = pshA.asBezier();
544 cairo_d2_sb(cr, A);
545 D2<SBasis> B = pshB.asBezier();
546 cairo_d2_sb(cr, B);
547 cairo_stroke(cr);
548
549 np_finder np(cr, A, B);
550 Path AP, BP;
551 AP.append(A); BP.append(B);
552 Crossings ip_list = curve_sweep<SimpleCrosser>(AP, BP);
553 if( ip_list.empty() )
554 {
555 np();
556 cairo_set_line_width (cr, 0.4);
557 cairo_set_source_rgba(cr, 0.7, 0.0, 0.7, 1.0);
558 cairo_move_to(cr, np.firstPoint());
559 cairo_line_to(cr, np.secondPoint());
560 cairo_stroke(cr);
561 //std::cerr << "np: (" << np.firstValue() << "," << np.secondValue() << ")" << std::endl;
562 }
563 Toy::draw(cr, notify, width, height, save,timer_stream);
564 }
565
566 public:
NearestPoints(unsigned int _A_bez_ord,unsigned int _B_bez_ord)567 NearestPoints(unsigned int _A_bez_ord, unsigned int _B_bez_ord)
568 : A_bez_ord(_A_bez_ord), B_bez_ord(_B_bez_ord)
569 {
570 handles.push_back(&pshA);
571 handles.push_back(&pshB);
572 for ( unsigned int i = 0; i < A_bez_ord; ++i )
573 pshA.push_back(Geom::Point(uniform()*400, uniform()*400));
574 for ( unsigned int i = 0; i < B_bez_ord; ++i )
575 pshB.push_back(Geom::Point(uniform()*400, uniform()*400));
576
577 }
578
579 private:
580 PointSetHandle pshA, pshB;
581 unsigned int A_bez_ord;
582 unsigned int B_bez_ord;
583 };
584
585
main(int argc,char ** argv)586 int main(int argc, char **argv)
587 {
588 unsigned int A_bez_ord=8;
589 unsigned int B_bez_ord=5;
590 if(argc > 2)
591 sscanf(argv[2], "%d", &B_bez_ord);
592 if(argc > 1)
593 sscanf(argv[1], "%d", &A_bez_ord);
594
595 init( argc, argv, new NearestPoints(A_bez_ord, B_bez_ord));
596 return 0;
597 }
598
599
600 /*
601 Local Variables:
602 mode:c++
603 c-file-style:"stroustrup"
604 c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
605 indent-tabs-mode:nil
606 fill-column:99
607 End:
608 */
609 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 :
610