Code-Eli  0.3.6
piecewise.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2 * Copyright (c) 2013 David D. Marshall <ddmarsha@calpoly.edu>
3 *
4 * All rights reserved. This program and the accompanying materials
5 * are made available under the terms of the Eclipse Public License v1.0
6 * which accompanies this distribution, and is available at
7 * http://www.eclipse.org/legal/epl-v10.html
8 *
9 * Contributors:
10 * David D. Marshall - initial code and implementation
11 ********************************************************************************/
12 
13 #ifndef eli_geom_curve_piecewise_hpp
14 #define eli_geom_curve_piecewise_hpp
15 
16 #include <map>
17 #include <iterator>
18 
19 #include "eli/code_eli.hpp"
20 
21 #include "eli/constants/math.hpp"
22 #include "eli/util/tolerance.hpp"
23 
25 
26 namespace eli
27 {
28  namespace geom
29  {
30  namespace utility
31  {
32  template<typename curve1__, typename curve2__, typename tol__>
33  bool check_point_continuity(const curve1__ &curve1, const typename curve1__::data_type &dt1,
34  const curve2__ &curve2, const typename curve2__::data_type &dt2,
35  const eli::geom::general::continuity &cont, const tol__ &tol)
36  {
37  switch(cont)
38  {
40  {
41  typename curve1__::point_type fppp1(curve1.fppp(1)); fppp1.normalize();
42  typename curve2__::point_type fppp2(curve2.fppp(0)); fppp2.normalize();
43 
44  if (!tol.approximately_equal(fppp1, fppp2))
45  return false;
46  else
47  return check_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::G2, tol);
48  break;
49  }
51  {
52  if (!tol.approximately_equal(curve1.fppp(1)/dt1/dt1/dt1, curve2.fppp(0)/dt2/dt2/dt2))
53  return false;
54  else
55  return check_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C2, tol);
56  break;
57  }
59  {
60  typename curve1__::point_type fpp1(curve1.fpp(1)); fpp1.normalize();
61  typename curve2__::point_type fpp2(curve2.fpp(0)); fpp2.normalize();
62 
63  if (!tol.approximately_equal(fpp1, fpp2))
64  return false;
65  else
66  return check_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::G1, tol);
67  break;
68  }
70  {
71  if (!tol.approximately_equal(curve1.fpp(1)/dt1/dt1, curve2.fpp(0)/dt2/dt2))
72  return false;
73  else
74  return check_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C1, tol);
75  break;
76  }
78  {
79  typename curve1__::point_type fp1(curve1.fp(1)); fp1.normalize();
80  typename curve2__::point_type fp2(curve2.fp(0)); fp2.normalize();
81 
82  if (!tol.approximately_equal(fp1, fp2))
83  return false;
84  else
85  return check_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::G0, tol);
86  break;
87  }
89  {
90  if (!tol.approximately_equal(curve1.fp(1)/dt1, curve2.fp(0)/dt2))
91  return false;
92  else
93  return check_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C0, tol);
94  break;
95  }
98  {
99  return tol.approximately_equal(curve1.f(1), curve2.f(0));
100  break;
101  }
103  {
104  return !tol.approximately_equal(curve1.f(1), curve2.f(0));
105  break;
106  }
107  default:
108  {
109  // shouldn't get here
110  assert(false);
111  return false;
112  break;
113  }
114  }
115 
116  // shouldn't get here
117  assert(false);
118  return false;
119  }
120 
121  namespace internal
122  {
123  template<typename curve1__, typename curve2__, typename tol__>
124  eli::geom::general::continuity report_point_continuity(const curve1__ &curve1, const typename curve1__::data_type &dt1,
125  const curve2__ &curve2, const typename curve2__::data_type &dt2,
126  const eli::geom::general::continuity &cont, const tol__ &tol)
127  {
128  typename curve1__::point_type v1;
129  typename curve2__::point_type v2;
130 
131  switch(cont)
132  {
134  {
135  v1=curve1.f(1);
136  v2=curve2.f(0);
137 
138  if (tol.approximately_equal(v1, v2))
139  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C0, tol);
140  else
141  return cont;
142 
143  break;
144  }
146  {
147  v1=curve1.fp(1)/dt1;
148  v2=curve2.fp(0)/dt2;
149 
150  if (tol.approximately_equal(v1, v2))
151  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C1, tol);
152  }
154  {
155  v1=curve1.fp(1).normalized();
156  v2=curve2.fp(0).normalized();
157 
158  if (tol.approximately_equal(v1, v2))
159  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::G1, tol);
160  else
161  return cont;
162 
163  break;
164  }
166  {
167  v1=curve1.fpp(1)/dt1/dt1;
168  v2=curve2.fpp(0)/dt2/dt2;
169 
170  if (tol.approximately_equal(v1, v2))
171  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C2, tol);
172  }
174  {
175  v1=curve1.fpp(1).normalized();
176  v2=curve2.fpp(0).normalized();
177 
178  if (tol.approximately_equal(v1, v2))
179  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::G2, tol);
180  else
181  return cont;
182 
183  break;
184  }
186  {
187  v1=curve1.fppp(1)/dt1/dt1/dt1;
188  v2=curve2.fppp(0)/dt2/dt2/dt2;
189 
190  if (tol.approximately_equal(v1, v2))
191  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::C3, tol);
192  }
194  {
195  v1=curve1.fppp(1).normalized();
196  v2=curve2.fppp(0).normalized();
197 
198  if (tol.approximately_equal(v1, v2))
199  return report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::G3, tol);
200  else
201  return cont;
202 
203  break;
204  }
207  {
208  return cont;
209  }
210  default:
211  {
212  // shouldn't get here
213  assert(false);
215  break;
216  }
217  }
218 
219  // shouldn't get here
220  assert(false);
222  }
223  }
224 
225  template<typename curve1__, typename curve2__, typename tol__>
226  eli::geom::general::continuity report_point_continuity(const curve1__ &curve1, const typename curve1__::data_type &dt1,
227  const curve2__ &curve2, const typename curve2__::data_type &dt2,
228  const tol__ &tol)
229  {
230  return internal::report_point_continuity(curve1, dt1, curve2, dt2, eli::geom::general::NOT_CONNECTED, tol);
231  }
232  }
233  }
234 }
235 
236 namespace eli
237 {
238  namespace geom
239  {
240 
241  namespace curve
242  {
243  template<template<typename, unsigned short, typename> class curve__, typename data__, unsigned short dim__, typename tol__ >
244  class piecewise;
245  }
246 
247  namespace intersect
248  {
249  template<template<typename, unsigned short, typename> class curve1__, typename data1__, unsigned short dim1__, typename tol1__>
255  }
256 
257  namespace curve
258  {
259  // forward declaration of length function used in methods below. The length function
260  // includes this header.
261  template<typename curve__>
262  void length(typename curve__::data_type &len, const curve__ &c, const typename curve__::data_type &tol);
263  template<typename curve__>
264  void length(typename curve__::data_type &len, const curve__ &c, const typename curve__::data_type &t0, const typename curve__::data_type &t1, const typename curve__::data_type &tol);
265 
266  template<template<typename, unsigned short, typename> class curve__, typename data__, unsigned short dim__, typename tol__=eli::util::tolerance<data__> >
267  class piecewise
268  {
269  public:
270  typedef curve__<data__, dim__, tol__> curve_type;
276  typedef data__ data_type;
277  typedef unsigned short dimension_type;
278  typedef tol__ tolerance_type;
280  {
288  };
289 
290  public:
291  piecewise() : tmax(0) {}
294 
296  {
297  if (this==&p)
298  return true;
299  if (tmax!=p.tmax)
300  return false;
301  if (tol!=p.tol)
302  return false;
303  if (number_segments()!=p.number_segments())
304  return false;
305  typename segment_collection_type::const_iterator scit, it;
306  for (scit=segments.begin(), it=p.segments.begin(); scit!=segments.end(); ++scit, ++it)
307  {
308  if ((*it)!=(*scit))
309  return false;
310  }
311 
312  return true;
313  }
314 
316  {
317  if (this==&p)
318  return (*this);
319  segments=p.segments;
320  tmax=p.tmax;
321  tol=p.tol;
322 
323  return (*this);
324  }
325 
327  {
328  return !operator==(p);
329  }
330 
331  static dimension_type dimension() {return dim__;}
332 
333  data_type get_tmax() const {return tmax;}
334 
335  data_type get_t0() const
336  {
337  return get_parameter_min();
338  }
339 
340  void set_t0(const data_type &t0_in)
341  {
342  if(!segments.empty())
343  {
344  if(t0_in != segments.begin()->first)
345  {
346  data_type t = t0_in;
347  segment_collection_type shiftseg;
348  for (typename segment_collection_type::iterator it=segments.begin(); it!=segments.end(); ++it)
349  {
350  data_type delta_t = get_delta_t(it);
351 
352  shiftseg.insert(shiftseg.end(), std::make_pair(t, it->second));
353 
354  t+=delta_t;
355  }
356  segments.swap(shiftseg);
357  tmax = t;
358  }
359  }
360  else
361  {
362  tmax=t0_in;
363  }
364  }
365 
366  data_type get_parameter_min() const
367  {
368  if(!segments.empty())
369  return segments.begin()->first;
370  else
371  return tmax;
372  }
373 
374  data_type get_parameter_max() const
375  {
376  return tmax;
377  }
378 
379  template<typename it__>
380  void get_parameters(it__ itt) const
381  {
382  typename segment_collection_type::const_iterator its;
383 
384  for (its=segments.begin(); its!=segments.end(); ++its)
385  {
386  (*itt)=its->first;++itt;
387  }
388  (*itt)=tmax;
389  }
390 
391  void parameter_report() const
392  {
393  printf("Parameter report:\n");
394  typename segment_collection_type::const_iterator it;
395 
396  int i = 0;
397  // cycle through all segments to get each bounding box to add
398  for (it=segments.begin(); it!=segments.end(); ++it)
399  {
400  printf(" seg: %d \t t: %f\n", i, it->first);
401  ++i;
402  }
403  printf(" tmax: %f\n", tmax);
404  printf("End report\n");
405  }
406 
407  void get_pmap( std::vector < data_type > &pmap )
408  {
409  pmap.clear();
410 
411  typename segment_collection_type::const_iterator it;
412  for (it=segments.begin(); it!=segments.end(); ++it)
413  {
414  pmap.push_back( it->first );
415  }
416  pmap.push_back( tmax );
417  }
418 
419  index_type number_segments() const {return static_cast<index_type>(segments.size());}
420 
421  void degree(index_type &mind, index_type &maxd) const
422  {
423  typename segment_collection_type::const_iterator it;
424 
425  it=segments.begin();
426 
427  index_type d = it->second.degree();
428  mind = d;
429  maxd = d;
430  ++it;
431 
432  // cycle through all segments to get each bounding box to add
433  for (; it!=segments.end(); ++it)
434  {
435  index_type d = it->second.degree();
436 
437  if(d<mind)
438  mind=d;
439  if(d>maxd)
440  maxd=d;
441  }
442  }
443 
444  template<typename it__>
445  void degrees(it__ itd)
446  {
447  typename segment_collection_type::const_iterator it;
448 
449  // cycle through all segments to get each degree
450  for (it=segments.begin(); it!=segments.end(); ++it, ++itd)
451  {
452  (*itd) = it->second.degree();
453  }
454  }
455 
456  void get_bounding_box(bounding_box_type &bb) const
457  {
458  typename segment_collection_type::const_iterator it;
459  bounding_box_type bb_local;
460 
461  bb.clear();
462 
463  // cycle through all segments to get each bounding box to add
464  for (it=segments.begin(); it!=segments.end(); ++it)
465  {
466  it->second.get_bounding_box(bb_local);
467  bb.add(bb_local);
468  }
469  }
470 
471  void rotate(const rotation_matrix_type &rmat)
472  {
473  typename segment_collection_type::iterator it;
474 
475  for (it=segments.begin(); it!=segments.end(); ++it)
476  {
477  it->second.rotate(rmat);
478  }
479  }
480 
481  void rotate(const rotation_matrix_type &rmat, const point_type &rorig)
482  {
483  typename segment_collection_type::iterator it;
484 
485  for (it=segments.begin(); it!=segments.end(); ++it)
486  {
487  it->second.rotate(rmat, rorig);
488  }
489  }
490 
491  void translate(const point_type &trans)
492  {
493  typename segment_collection_type::iterator it;
494 
495  for (it=segments.begin(); it!=segments.end(); ++it)
496  {
497  it->second.translate(trans);
498  }
499  }
500 
501  bool closed() const
502  {
503  typename segment_collection_type::const_iterator itlast, itfirst;
504 
505  itlast=segments.end(); --itlast;
506  itfirst=segments.begin();
507 
508  data_type dtlast = get_delta_t(itlast);
509  data_type dtfirst = get_delta_t(itfirst);
510 
511  return eli::geom::utility::check_point_continuity(itlast->second, dtlast, itfirst->second, dtfirst, eli::geom::general::C0, tol);
512  }
513 
514  bool open() const
515  {
516  return !closed();
517  }
518 
519  void reverse()
520  {
522  typename segment_collection_type::iterator itr;
523  typename segment_collection_type::iterator itrguess = rseg.begin();
524 
525  data_type t(get_parameter_min());
526 
527  for (typename segment_collection_type::reverse_iterator it=segments.rbegin(); it!=segments.rend(); ++it)
528  {
529  itr = rseg.insert(itrguess, std::make_pair(t, it->second));
530 
531  // reverse each segment
532  itr->second.reverse();
533 
534  data_type delta_t = get_delta_t(it);
535  t += delta_t;
536 
537  itrguess = itr;
538  }
539  segments.swap(rseg);
540 
541  // Parametric length should stay the same.
542  assert(t == tmax);
543 
544  // check if still connected
546  }
547 
548 
549  void reflect_xy()
550  {
551  typename segment_collection_type::iterator it;
552 
553  for (it=segments.begin(); it!=segments.end(); ++it)
554  {
555  it->second.reflect_xy();
556  }
557  }
558 
559  void reflect_xz()
560  {
561  typename segment_collection_type::iterator it;
562 
563  for (it=segments.begin(); it!=segments.end(); ++it)
564  {
565  it->second.reflect_xz();
566  }
567  }
568 
569  void reflect_yz()
570  {
571  typename segment_collection_type::iterator it;
572 
573  for (it=segments.begin(); it!=segments.end(); ++it)
574  {
575  it->second.reflect_yz();
576  }
577  }
578 
579  void reflect(const point_type &normal)
580  {
581  typename segment_collection_type::iterator it;
582 
583  for (it=segments.begin(); it!=segments.end(); ++it)
584  {
585  it->second.reflect(normal);
586  }
587  }
588 
589  void reflect(const point_type &normal, const data_type &d)
590  {
591  typename segment_collection_type::iterator it;
592 
593  for (it=segments.begin(); it!=segments.end(); ++it)
594  {
595  it->second.reflect(normal, d);
596  }
597  }
598 
599  void clear() {segments.clear();}
600 
601  template<typename it__>
602  error_code set(it__ itb, it__ ite)
603  {
604  segments.clear();
605 
606  index_type i;
607  it__ it;
608  for (i=0, it=itb; it!=ite; ++i, ++it)
609  {
610  error_code err=push_back(*it);
611  if (err!=NO_ERRORS)
612  {
613  segments.clear();
614  return err;
615  }
616  }
617 
619 
620  return NO_ERRORS;
621  }
622 
623  template<typename it__, typename itd__>
624  error_code set(it__ itb, it__ ite, itd__ itd)
625  {
626  segments.clear();
627 
628  index_type i;
629  it__ it;
630  itd__ itdt;
631  for (i=0, it=itb, itdt=itd; it!=ite; ++i, ++it, ++itdt)
632  {
633  error_code err=push_back(*it, *itdt);
634  if (err!=NO_ERRORS)
635  {
636  segments.clear();
637  return err;
638  }
639  }
640 
642 
643  return NO_ERRORS;
644  }
645 
646  error_code push_front(const curve_type &curve, const data_type &dt=1.0)
647  {
648  if (dt<=0)
650 
651  // check to make sure have valid segments
652  if (!segments.empty())
653  {
654  data_type start_dt = get_delta_t(segments.begin());
655  if (!eli::geom::utility::check_point_continuity(curve, dt, segments.begin()->second, start_dt, eli::geom::general::C0, tol))
656  {
657  return SEGMENT_NOT_CONNECTED;
658  }
659  }
660 
661  // add segment
662  data_type t0(get_parameter_min());
663  t0 -= dt;
664  segments.insert(segments.begin(), std::make_pair(t0, curve));
665 
667 
668  return NO_ERRORS;
669  }
670 
672  {
673  typename segment_collection_type::const_reverse_iterator itp;
674  error_code err;
675 
676  for (itp=p.segments.rbegin(); itp!=p.segments.rend(); ++itp)
677  {
678  err=push_front(itp->second, p.get_delta_t(itp));
679  if (err!=NO_ERRORS)
680  {
681  return err;
682  }
683  }
684 
685  return NO_ERRORS;
686  }
687 
688  error_code push_back(const curve_type &curve, const data_type &dt=1.0)
689  {
690  if (dt<=0)
692 
693  // check to make sure have valid segments
694  if (!segments.empty())
695  {
696  data_type end_dt = get_delta_t(segments.rbegin());
697  if (!eli::geom::utility::check_point_continuity(segments.rbegin()->second, end_dt, curve, dt, eli::geom::general::C0, tol))
698  {
699  return SEGMENT_NOT_CONNECTED;
700  }
701  }
702 
703  // add segment
704  segments.insert(segments.end(), std::make_pair(tmax, curve));
705  tmax+=dt;
706 
708 
709  return NO_ERRORS;
710  }
711 
713  {
714  typename segment_collection_type::const_iterator itp;
715  error_code err;
716 
717  for (itp=p.segments.begin(); itp!=p.segments.end(); ++itp)
718  {
719  err=push_back(itp->second, p.get_delta_t(itp));
720  if (err!=NO_ERRORS)
721  {
722  return err;
723  }
724  }
725 
726  return NO_ERRORS;
727  }
728 
729  error_code get(curve_type &curve, const index_type &index) const
730  {
731  data_type dt;
732  return get(curve, dt, index);
733  }
734 
736  {
737  typename segment_collection_type::const_iterator scit;
738  for (scit=segments.begin(); scit!=segments.end(); ++scit)
739  {
740  scit->second.degree_promote();
741  }
742 
743  return NO_ERRORS;
744  }
745 
746  error_code degree_promote(const index_type &index)
747  {
748  if (index>=number_segments())
749  return INVALID_INDEX;
750 
751  typename segment_collection_type::iterator scit;
752  find_segment(scit, index);
753 
754  scit->second.degree_promote();
755  return NO_ERRORS;
756  }
757 
758  error_code degree_promote_to(const index_type &deg)
759  {
760  typename segment_collection_type::const_iterator scit;
761  for (scit=segments.begin(); scit!=segments.end(); ++scit)
762  {
763  scit->second.degree_promote_to(deg);
764  }
765 
766  return NO_ERRORS;
767  }
768 
769  error_code degree_promote_to(const index_type &index, const index_type &deg)
770  {
771  if (index>=number_segments())
772  return INVALID_INDEX;
773 
774  typename segment_collection_type::iterator scit;
775  find_segment(scit, index);
776 
777  scit->second.degree_promote_to(deg);
778  return NO_ERRORS;
779  }
780 
781  error_code get(curve_type &curve, data_type &dt, const index_type &index) const
782  {
783  if (index>=number_segments())
784  return INVALID_INDEX;
785 
786  // advance to desired index
787  index_type i;
788  typename segment_collection_type::const_iterator scit;
789  for (i=0, scit=segments.begin(); i<index; ++i, ++scit) {}
790 
791  curve=scit->second;
792  dt=get_delta_t(scit);
793  return NO_ERRORS;
794  }
795 
796  error_code replace(const curve_type &curve, const index_type &index)
797  {
798  if (index>=number_segments())
799  return INVALID_INDEX;
800 
801  typename segment_collection_type::iterator scit;
802  data_type dt, dto;
803 
804  find_segment(scit, index);
805 
806  dt = get_delta_t(scit);
807 
808  typename segment_collection_type::iterator scito;
809 
810  // check the connectivity on adjacent nodes (if available)
811  if (index>0)
812  {
813  scito=scit;
814  --scito;
815  dto = get_delta_t(scito);
816  if (!eli::geom::utility::check_point_continuity(scito->second, dto, curve, dt, eli::geom::general::C0, tol))
817  {
818  return SEGMENT_NOT_CONNECTED;
819  }
820  }
821  if ((index+1)<number_segments())
822  {
823  scito=scit;
824  ++scito;
825  dto = get_delta_t(scito);
826  if (!eli::geom::utility::check_point_continuity(curve, dt, scito->second, dto, eli::geom::general::C0, tol))
827  {
828  return SEGMENT_NOT_CONNECTED;
829  }
830  }
831 
832  // set the new curve
833  scit->second=curve;
834 
836 
837  return NO_ERRORS;
838  }
839 
840  error_code replace(const curve_type &curve, const index_type &index0, const index_type &index1)
841  {
842  if (index0>=number_segments())
843  return INVALID_INDEX;
844  if (index1>number_segments())
845  return INVALID_INDEX;
846  if (index0>=index1)
847  return INVALID_INDEX;
848 
849  typename segment_collection_type::iterator scit0, scit1, scito;
850  find_segment(scit0, index0);
851  find_segment(scit1, index1);
852 
853  data_type dt0, dt1, dto;
854  dt0 = get_delta_t(scit0);
855  dt1 = get_delta_t(scit1);
856 
857  // check the connectivity on adjacent nodes (if available)
858  if (index0>0)
859  {
860  scito=scit0;
861  --scito;
862  dto = get_delta_t(scito);
863  if (!eli::geom::utility::check_point_continuity(scito->second, dto, curve, dt0, eli::geom::general::C0, tol))
864  {
865  return SEGMENT_NOT_CONNECTED;
866  }
867  }
868  if ((index1+1)<number_segments())
869  {
870  scito=scit1;
871  dto = get_delta_t(scito);
872  if (!eli::geom::utility::check_point_continuity(curve, dt1, scito->second, dto, eli::geom::general::C0, tol))
873  {
874  return SEGMENT_NOT_CONNECTED;
875  }
876  }
877 
878  data_type t = scit0->first;
879 
880  typename segment_collection_type::iterator itguess;
881 
882  // erase old segments
883  itguess = segments.erase(scit0, scit1);
884 
885  segments.insert(itguess, std::make_pair(t, curve));
886 
888 
889  return NO_ERRORS;
890  }
891 
892  error_code replace(const piecewise<curve__, data_type, dim__> &p, const index_type &index)
893  {
894  if (index>=number_segments())
895  return INVALID_INDEX;
896 
897  typename segment_collection_type::iterator scit, scito;
898  find_segment(scit, index);
899 
900  // Find parameter span to insert
901  data_type pt0(p.get_parameter_min()), ptmax(p.get_parameter_max()), ptspan(ptmax - pt0);
902 
903  // get the first and last curve to insert
904  typename segment_collection_type::const_iterator itps, itpe;
905  curve_type cs, ce;
906  data_type dts, dte;
907 
908  itps = p.segments.begin();
909  cs = itps->second;
910  dts = p.get_delta_t(itps);
911 
912  itpe = p.segments.end(); --itpe;
913  ce = itpe->second;
914  dte = p.get_delta_t(itpe);
915 
916  // Find parameter span of segment to replace
917  data_type dti, dto;
918  dti = get_delta_t(scit);
919 
920  // Ratio of parameter lengths, replace/insert
921  data_type pratio = dti/ptspan;
922 
923  // check the connectivity on adjacent nodes (if available)
924  if (index>0)
925  {
926  scito=scit;
927  --scito;
928  dto = get_delta_t(scito);
929  if (!eli::geom::utility::check_point_continuity(scito->second, dto, cs, dts*pratio, eli::geom::general::C0, tol))
930  {
931  return SEGMENT_NOT_CONNECTED;
932  }
933  }
934  if ((index+1)<number_segments())
935  {
936  scito=scit;
937  ++scito;
938  dto = get_delta_t(scito);
939  if (!eli::geom::utility::check_point_continuity(ce, dte*pratio, scito->second, dto, eli::geom::general::C0, tol))
940  {
941  return SEGMENT_NOT_CONNECTED;
942  }
943  }
944 
945  typename segment_collection_type::const_iterator it=itps;
946  typename segment_collection_type::iterator itguess = scit;
947 
948  data_type dtp = dts;
949 
950  data_type t = scit->first;
951 
952  // Substitute first curve
953  scit->second = it->second;
954 
955  t += dtp*pratio;
956 
957  for(++it; it != p.segments.end(); ++it)
958  {
959  itguess = segments.insert(itguess, std::make_pair(t, it->second));
960  dtp = p.get_delta_t(it);
961  t += dtp*pratio;
962  }
963 
965 
966  return NO_ERRORS;
967  }
968 
973  error_code replace(const piecewise<curve__, data_type, dim__> &p, const index_type &index0, const index_type &index1)
974  {
975  if (index0>=number_segments())
976  return INVALID_INDEX;
977  if (index1>number_segments())
978  return INVALID_INDEX;
979  if (index0>=index1)
980  return INVALID_INDEX;
981 
982  typename segment_collection_type::iterator scit0, scit1, scito;
983  find_segment(scit0, index0);
984  find_segment(scit1, index1);
985 
986  // Find parameter span to insert
987  data_type pt0(p.get_parameter_min()), ptmax(p.get_parameter_max()), ptspan(ptmax - pt0);
988 
989  // get the first and last curve to insert
990  typename segment_collection_type::const_iterator itps, itpe;
991  curve_type cs, ce;
992  data_type dts, dte;
993 
994  itps = p.segments.begin();
995  cs = itps->second;
996  dts = p.get_delta_t(itps);
997 
998  itpe = p.segments.end(); --itpe;
999  ce = itpe->second;
1000  dte = p.get_delta_t(itpe);
1001 
1002  // Find parameter span of segment to replace
1003  data_type dti, dto;
1004 
1005  if(scit1 != segments.end())
1006  dti = scit1->first - scit0->first;
1007  else
1008  dti = tmax - scit0->first;
1009 
1010  // Ratio of parameter lengths, replace/insert
1011  data_type pratio = dti/ptspan;
1012 
1013  // check the connectivity on adjacent nodes (if available)
1014  if (index0>0)
1015  {
1016  scito=scit0;
1017  --scito;
1018  dto = get_delta_t(scito);
1019  if (!eli::geom::utility::check_point_continuity(scito->second, dto, cs, dts*pratio, eli::geom::general::C0, tol))
1020  {
1021  return SEGMENT_NOT_CONNECTED;
1022  }
1023  }
1024  if (index1<number_segments())
1025  {
1026  scito=scit1;
1027  dto = get_delta_t(scito);
1028  if (!eli::geom::utility::check_point_continuity(ce, dte*pratio, scito->second, dto, eli::geom::general::C0, tol))
1029  {
1030  return SEGMENT_NOT_CONNECTED;
1031  }
1032  }
1033 
1034  data_type t = scit0->first;
1035  data_type dtp;
1036 
1037  typename segment_collection_type::const_iterator it;
1038  typename segment_collection_type::iterator itguess;
1039 
1040  // erase old segments
1041  itguess = segments.erase(scit0, scit1);
1042 
1043  for(it=p.segments.begin(); it != p.segments.end(); ++it)
1044  {
1045  itguess = segments.insert(itguess, std::make_pair(t, it->second));
1046  dtp = p.get_delta_t(it);
1047  t += dtp*pratio;
1048  }
1049 
1051 
1052  return NO_ERRORS;
1053  }
1054 
1055  error_code split(const data_type &t)
1056  {
1057  // find segment that corresponds to given t
1058  typename segment_collection_type::iterator it;
1059  data_type tt;
1060  find_segment(it, tt, t);
1061 
1062  // do some checking to see if even need to split
1063  if (tol.approximately_equal(tt, 0))
1064  return NO_ERRORS;
1065  if (tol.approximately_equal(tt, 1))
1066  return NO_ERRORS;
1067  if (it==segments.end())
1068  return INVALID_PARAM;
1069 
1070  // split the segment and replace
1071  return split_seg(it, tt);
1072  }
1073 
1075  {
1076  // find segment that corresponds to given t
1077  typename segment_collection_type::const_iterator it, iit;
1078  data_type tt;
1079  error_code ec;
1080 
1081  find_segment(it, tt, tsplit);
1082 
1083  // do some checking to see if even need to split
1084  if (it==segments.end())
1085  return INVALID_PARAM;
1086 
1087  // if found end of segment adjust to make beginning of next
1088  if (tol.approximately_equal(tt, 1))
1089  {
1090  ++it;
1091  tt=0;
1092 
1093  // catch case that wanted to split at end
1094  if (it==segments.end())
1095  {
1096  before=(*this);
1097  after.clear();
1098  return NO_ERRORS;
1099  }
1100  }
1101 
1102  // catch case that wanted to split at beginning
1103  if ((tol.approximately_equal(tt, 0)) && (it==segments.begin()))
1104  {
1105  before.clear();
1106  after=(*this);
1107  return NO_ERRORS;
1108  }
1109 
1110  // fill first half
1111  before.clear();
1112  after.clear();
1113  before.set_t0(get_t0());
1114  for (iit=segments.cbegin(); iit!=it; ++iit)
1115  {
1116  ec=before.push_back(iit->second, get_delta_t(iit));
1117  if (ec!=NO_ERRORS)
1118  {
1119  before.clear();
1120  assert(false);
1121  return ec;
1122  }
1123  }
1124 
1125  // split the segment and replace (if needed)
1126  if (!tol.approximately_equal(tt, 0))
1127  {
1128  data_type delta_t(get_delta_t(it));
1129  curve_type cl, cr;
1130 
1131  // split underlying curve and add left to before and right to after
1132  it->second.split(cl, cr, tt);
1133  ec=before.push_back(cl, tt*delta_t);
1134  if (ec!=NO_ERRORS)
1135  {
1136  before.clear();
1137  assert(false);
1138  return ec;
1139  }
1140 
1141  after.set_t0(it->first+tt*delta_t);
1142  after.push_back(cr, (1-tt)*delta_t);
1143  }
1144  else
1145  {
1146  after.set_t0(it->first);
1147  after.push_back(it->second, get_delta_t(it));
1148  }
1149 
1150  // fill second half
1151  iit=it;
1152  for (++iit; iit!=segments.cend(); ++iit)
1153  {
1154  ec=after.push_back(iit->second, get_delta_t(iit));
1155  if (ec!=NO_ERRORS)
1156  {
1157  before.clear();
1158  after.clear();
1159  assert(false);
1160  return ec;
1161  }
1162  }
1163 
1164  return NO_ERRORS;
1165  }
1166 
1167  void to_cubic(const data_type &ttol)
1168  {
1169  typename segment_collection_type::iterator it;
1170  for (it=segments.begin(); it!=segments.end(); ++it)
1171  segment_to_cubic(it, ttol);
1172  }
1173 
1174  void round(const data_type &rad)
1175  {
1176  // catch special case of no rounding
1177  if (rad<=0)
1178  {
1179  return;
1180  }
1181 
1182  // Note: this could be implemented more efficiently if wanted to track the
1183  // segment container iterators, but that would require more code
1184  // duplication since the round(rad, i) method calls other methods with
1185  // useful error checking.
1186 
1187  // Note: need to keep calling number_segments() because a call to round(rad, i)
1188  // might increase the number of sections and need to make sure that this
1189  // loop gets to the last segment
1190  for (index_type i=0; i<=number_segments(); ++i)
1191  {
1192  round(rad, i);
1193  }
1194  }
1195 
1196  bool round(const data_type &rad, const index_type &joint)
1197  {
1198  // if joint doesn't exist then return
1199  if ((joint<0) || (joint>number_segments()))
1200  {
1201  assert(false);
1202  return false;
1203  }
1204 
1205  // catch special case of no rounding
1206  if (rad<=0)
1207  {
1208  return false;
1209  }
1210 
1211  // catch special case of rounding first or last joint of open curve
1212  if (((joint==0) || (joint==number_segments())) && open())
1213  {
1214  return false;
1215  }
1216 
1217  index_type im1, i;
1218  bool rounding_end(false);
1219 
1220  if ((joint==0) || (joint==number_segments()))
1221  {
1222  i=0;
1223  im1=number_segments()-1;
1224  rounding_end=true;
1225  }
1226  else
1227  {
1228  i=joint;
1229  im1=joint-1;
1230  }
1231 
1232  curve_type cim1, ci, arc, c, ctrim;
1233  data_type dtim1(0), dti(0), r, lenim1, leni, tim1_split(-1), ti_split(-1);
1234  point_type fim1, fi, fpim1, fpi;
1235  control_point_type cp[4];
1236 
1237  // get the two curve segments
1238  get(cim1, dtim1, im1);
1239  get(ci, dti, i);
1240 
1241  // check to see if joint needs to be rounded
1242  fpim1=cim1.fp(1); fpim1.normalize();
1243  fpi=ci.fp(0); fpi.normalize();
1244  if (tol.approximately_equal(fpi.dot(fpim1), 1))
1245  {
1246  return false;
1247  }
1248 
1249  // determine what the actual radius will be
1250  r=rad;
1251  eli::geom::curve::length(lenim1, cim1, tol.get_absolute_tolerance());
1252  eli::geom::curve::length(leni, ci, tol.get_absolute_tolerance());
1253  if (lenim1<r)
1254  {
1255  tim1_split=0;
1256  r=lenim1;
1257  }
1258  if (leni<r)
1259  {
1260  tim1_split=-1;
1261  ti_split=1;
1262  r=leni;
1263  }
1264 
1265  // find coordinate that corresponds to location of radius on each curve
1266  double small_t(1e-3);
1267  if (tim1_split<0)
1268  {
1269  // FIX: this is exact for straight lines and approximate for other curves
1270  tim1_split=1-r/lenim1;
1271 
1272  // if resulting segment is too small then make entire edge the round
1273  if (tim1_split<small_t)
1274  {
1275  tim1_split=0;
1276  }
1277  }
1278  if (ti_split<0)
1279  {
1280  // FIX: this is exact for straight lines and approximate for other curves
1281  ti_split=r/leni;
1282 
1283  // if resulting segment is too small then make entire edge the round
1284  if (ti_split>(1-small_t))
1285  {
1286  ti_split=1;
1287  }
1288  }
1289 
1290  // if resulting round is too small compared then don't round
1291  if ((tim1_split>(1-small_t)) || (ti_split<small_t))
1292  return false;
1293 
1294  // calculate the points & slopes for end of round
1295  fim1=cim1.f(tim1_split);
1296  fpim1=cim1.fp(tim1_split); fpim1.normalize();
1297  fi=ci.f(ti_split);
1298  fpi=ci.fp(ti_split); fpi.normalize();
1299 
1300  // build round curve
1301  data_type k=static_cast<data_type>(4)*(eli::constants::math<data_type>::sqrt_two()-static_cast<data_type>(1))/static_cast<data_type>(3); // use this value so that 90 deg. corners will have close circle
1302  arc.resize(3);
1303  cp[0]=fim1;
1304  cp[1]=fim1+fpim1*(k*r);
1305  cp[2]=fi-fpi*(k*r);
1306  cp[3]=fi;
1307  arc.set_control_point(cp[0], 0);
1308  arc.set_control_point(cp[1], 1);
1309  arc.set_control_point(cp[2], 2);
1310  arc.set_control_point(cp[3], 3);
1311 
1312  // split the two curves
1313  if (tim1_split>0)
1314  {
1315  cim1.split(c, ctrim, tim1_split); cim1=c;
1316  }
1317  if (ti_split<1)
1318  {
1319  ci.split(ctrim, c, ti_split); ci=c;
1320  }
1321 
1322  // replace/add curves
1323  error_code ec;
1324  if (rounding_end)
1325  {
1326  curve_type arc1, arc2;
1328 
1329  // split the arc
1330  arc.split(arc1, arc2, static_cast<data_type>(0.5));
1331 
1332  // put the ith segment and arc2 onto the end of the list of curves
1333  ec=pct0.push_back(cim1, dtim1*tim1_split);
1334  if (ec!=NO_ERRORS)
1335  {
1336  assert(false);
1337  return false;
1338  }
1339  ec=pct0.push_back(arc1, dtim1*(static_cast<data_type>(1)-tim1_split));
1340  if (ec!=NO_ERRORS)
1341  {
1342  assert(false);
1343  return false;
1344  }
1345 
1346  ec=replace(pct0, number_segments()-1);
1347  if (ec!=NO_ERRORS)
1348  {
1349  assert(false);
1350  return false;
1351  }
1352 
1353  // put arc1 and the (i-1)st segment onto the front of the list of curves
1354  ec=pct1.push_front(ci, dti*(1-ti_split));
1355  if (ec!=NO_ERRORS)
1356  {
1357  assert(false);
1358  return false;
1359  }
1360  ec=pct1.push_front(arc2, dti*ti_split);
1361  if (ec!=NO_ERRORS)
1362  {
1363  assert(false);
1364  return false;
1365  }
1366 
1367  ec=replace(pct1, 0);
1368  if (ec!=NO_ERRORS)
1369  {
1370  assert(false);
1371  return false;
1372  }
1373  }
1374  else
1375  {
1377 
1378  if (tim1_split>0)
1379  {
1380  ec=pct.push_back(cim1, dtim1*tim1_split);
1381  if (ec!=NO_ERRORS)
1382  {
1383  assert(false);
1384  return false;
1385  }
1386  }
1387  ec=pct.push_back(arc, dtim1*(1-tim1_split)+dti*ti_split);
1388  if (ec!=NO_ERRORS)
1389  {
1390  assert(false);
1391  return false;
1392  }
1393  if (ti_split<1)
1394  {
1395  ec=pct.push_back(ci, dti*(1-ti_split));
1396  if (ec!=NO_ERRORS)
1397  {
1398  assert(false);
1399  return false;
1400  }
1401  }
1402 
1403  // replace the two segments with the piecewise curve
1404  ec=replace(pct, i-1, i+1);
1405  if (ec!=NO_ERRORS)
1406  {
1407  assert(false);
1408  return false;
1409  }
1410  }
1411 
1412  return true;
1413  }
1414 
1415  bool continuous(eli::geom::general::continuity cont, const data_type &t) const
1416  {
1417  // find segment that corresponds to given t
1418  typename segment_collection_type::const_iterator it, itfirst, itsecond;
1419  data_type tt(0);
1420  find_segment(it, tt, t);
1421 
1422  if (it==segments.end())
1423  {
1424  assert(false);
1425  return false;
1426  }
1427 
1428  if (tt==0)
1429  {
1430  if (it==segments.begin())
1431  {
1432  if (open())
1433  {
1434  return (cont==eli::geom::general::NOT_CONNECTED);
1435  }
1436  else
1437  {
1438  typename segment_collection_type::const_iterator itlast(segments.end());
1439 
1440  --itlast;
1441  itfirst=itlast;
1442  itsecond=it;
1443  }
1444  }
1445  else
1446  {
1447  itsecond=it;
1448  itfirst=it; --itfirst;
1449  }
1450  }
1451  else if (tt==1)
1452  {
1453  typename segment_collection_type::const_iterator itlast(segments.end());
1454 
1455  --itlast;
1456  if (it==itlast)
1457  {
1458  if (open())
1459  {
1460  return (cont==eli::geom::general::NOT_CONNECTED);
1461  }
1462  else
1463  {
1464  itfirst=it;
1465  itsecond=segments.begin();
1466  }
1467  }
1468  else
1469  {
1470  itfirst=it;
1471  itsecond=it; ++itsecond;
1472  }
1473  }
1474  else
1475  {
1476  return (cont!=eli::geom::general::NOT_CONNECTED);
1477  }
1478 
1479  data_type dtfirst = get_delta_t(itfirst);
1480  data_type dtsecond = get_delta_t(itsecond);
1481 
1482  // check the continuity of the two sections
1483  return eli::geom::utility::check_point_continuity(itfirst->second, dtfirst, itsecond->second, dtsecond, cont, tol);
1484  }
1485 
1486  eli::geom::general::continuity continuity(const data_type &t) const
1487  {
1488  // find segment that corresponds to given t
1489  typename segment_collection_type::const_iterator it, itfirst, itsecond;
1490  data_type tt(0);
1491  find_segment(it, tt, t);
1492 
1493  if (it==segments.end())
1494  {
1495  assert(false);
1497  }
1498 
1499  if (tt==0)
1500  {
1501  if (it==segments.begin())
1502  {
1503  if (open())
1504  {
1506  }
1507  else
1508  {
1509  typename segment_collection_type::const_iterator itlast(segments.end());
1510 
1511  --itlast;
1512  itfirst=itlast;
1513  itsecond=it;
1514  }
1515  }
1516  else
1517  {
1518  itsecond=it;
1519  itfirst=it; --itfirst;
1520  }
1521  }
1522  else if (tt==1)
1523  {
1524  typename segment_collection_type::const_iterator itlast(segments.end());
1525 
1526  --itlast;
1527  if (it==itlast)
1528  {
1529  if (open())
1530  {
1532  }
1533  else
1534  {
1535  itfirst=it;
1536  itsecond=segments.begin();
1537  }
1538  }
1539  else
1540  {
1541  itfirst=it;
1542  itsecond=it; ++itsecond;
1543  }
1544  }
1545  else
1546  {
1548  }
1549 
1550  data_type dtfirst = get_delta_t(itfirst);
1551  data_type dtsecond = get_delta_t(itsecond);
1552 
1553  // check the continuity of the two sections
1554  return eli::geom::utility::report_point_continuity(itfirst->second, dtfirst, itsecond->second, dtsecond, tol);
1555  }
1556 
1557  bool smooth(const data_type &angle_tol, const data_type &t) const
1558  {
1559  // find segment that corresponds to given t
1560  typename segment_collection_type::const_iterator it, itfirst, itsecond;
1561  data_type tt(0);
1562  find_segment(it, tt, t);
1563 
1564  if (it==segments.end())
1565  {
1566  assert(false);
1567  return false;
1568  }
1569 
1570  if (tt==0)
1571  {
1572  if (it==segments.begin())
1573  {
1574  if (open())
1575  {
1576  return false;
1577  }
1578  else
1579  {
1580  typename segment_collection_type::const_iterator itlast(segments.end());
1581 
1582  --itlast;
1583  itfirst=itlast;
1584  itsecond=it;
1585  }
1586  }
1587  else
1588  {
1589  itsecond=it;
1590  itfirst=it; --itfirst;
1591  }
1592  }
1593  else if (tt==1)
1594  {
1595  typename segment_collection_type::const_iterator itlast(segments.end());
1596 
1597  --itlast;
1598  if (it==itlast)
1599  {
1600  if (open())
1601  {
1602  return false;
1603  }
1604  else
1605  {
1606  itfirst=it;
1607  itsecond=segments.begin();
1608  }
1609  }
1610  else
1611  {
1612  itfirst=it;
1613  itsecond=it; ++itsecond;
1614  }
1615  }
1616  else
1617  {
1618  return true;
1619  }
1620 
1621  data_type dtfirst = get_delta_t(itfirst);
1622  data_type dtsecond = get_delta_t(itsecond);
1623 
1624  // check if coincident points
1625  if (eli::geom::utility::check_point_continuity(itfirst->second, dtfirst, itsecond->second, dtsecond, eli::geom::general::C0, tol))
1626  {
1627  // check if angles between fp's are less than angle
1628  point_type fp1(itfirst->second.fp(1));
1629  point_type fp2(itsecond->second.fp(0));
1630  data_type val;
1631 
1632  val=fp1.norm();
1633  // NOTE: This is a case where the geometric tolerance object should catch this
1634  if (val>tol.get_relative_tolerance())
1635  {
1636  fp1/=val;
1637  }
1638  else
1639  {
1640  fp1.setZero();
1641  // catch case where both vectors are nearly zero
1642  if (tol.approximately_equal(fp1, fp2))
1643  {
1644  return true;
1645  }
1646  }
1647  val=fp2.norm();
1648  // NOTE: This is a case where the geometric tolerance object should catch this
1649  if (val>tol.get_relative_tolerance())
1650  {
1651  fp2/=val;
1652  }
1653  else
1654  {
1655  fp2.setZero();
1656  // since other vector was non zero (otherwise previous else case would have triggered)
1657  if (tol.approximately_equal(fp1, fp2))
1658  {
1659  return true;
1660  }
1661  // know the vectors are not in same direction
1662  else
1663  {
1664  return false;
1665  }
1666  }
1667 
1668  val=std::abs(1-fp1.dot(fp2));
1669  return (val<=angle_tol);
1670  }
1671 
1672  // check the continuity of the two sections
1673  return false;
1674  }
1675 
1676  void find_discontinuities(eli::geom::general::continuity cont, std::vector<data_type> &tdisc) const
1677  {
1678  // clear input vector
1679  tdisc.clear();
1680 
1681  index_type i, istart(0), njoints(number_segments()+1);
1682  std::vector<data_type> joints;
1683 
1684  // get all of the joints
1685  get_parameters(std::back_inserter(joints));
1686 
1687  // if curve is open then don't check last joint
1688  if (open())
1689  {
1690  ++istart;
1691  --njoints;
1692  }
1693 
1694  // check each joint (after starting joint) if it is continuous
1695  for (i=istart; i<njoints; ++i)
1696  {
1697  if (!continuous(cont, joints[i]))
1698  {
1699  tdisc.push_back(joints[i]);
1700  }
1701  }
1702  }
1703 
1704  void find_discontinuities(const data_type &angle_tol, std::vector<data_type> &tdisc) const
1705  {
1706  // clear input vector
1707  tdisc.clear();
1708 
1709  index_type i, istart(0), njoints(number_segments()+1);
1710  std::vector<data_type> joints;
1711 
1712  // get all of the joints
1713  get_parameters(std::back_inserter(joints));
1714 
1715  // if curve is open then don't check last joint
1716  if (open())
1717  {
1718  ++istart;
1719  --njoints;
1720  }
1721 
1722  // check each joint (after starting joint) if it is continuous
1723  for (i=istart; i<njoints; ++i)
1724  {
1725  if (!smooth(angle_tol, joints[i]))
1726  {
1727  tdisc.push_back(joints[i]);
1728  }
1729  }
1730  }
1731 
1732  point_type f(const data_type &t) const
1733  {
1734  // find segment that corresponds to given t
1735  typename segment_collection_type::const_iterator it;
1736  data_type tt(0);
1737  find_segment(it, tt, t);
1738 
1739  if (it==segments.end())
1740  {
1741  assert(false);
1742  --it;
1743  }
1744 
1745  return it->second.f(tt);
1746  }
1747 
1748  point_type fp(const data_type &t) const
1749  {
1750  // find segment that corresponds to given t
1751  typename segment_collection_type::const_iterator it;
1752  data_type tt, delta_t;
1753  find_segment(it, tt, t);
1754 
1755  if (it==segments.end())
1756  {
1757  assert(false);
1758  --it;
1759  }
1760 
1761  delta_t = get_delta_t(it);
1762  return it->second.fp(tt)/delta_t;
1763  }
1764 
1765  point_type fpp(const data_type &t) const
1766  {
1767  // find segment that corresponds to given t
1768  typename segment_collection_type::const_iterator it;
1769  data_type tt, delta_t;
1770  find_segment(it, tt, t);
1771 
1772  if (it==segments.end())
1773  {
1774  assert(false);
1775  --it;
1776  }
1777 
1778  delta_t = get_delta_t(it);
1779  return it->second.fpp(tt)/(delta_t*delta_t);
1780  }
1781 
1782  point_type fppp(const data_type &t) const
1783  {
1784  // find segment that corresponds to given t
1785  typename segment_collection_type::const_iterator it;
1786  data_type tt, delta_t;
1787  find_segment(it, tt, t);
1788 
1789  if (it==segments.end())
1790  {
1791  assert(false);
1792  --it;
1793  }
1794 
1795  delta_t = get_delta_t(it);
1796  return it->second.fppp(tt)/(delta_t*delta_t*delta_t);
1797  }
1798 
1799  point_type tanget(const data_type &t) const
1800  {
1801  // find segment that corresponds to given t
1802  typename segment_collection_type::const_iterator it;
1803  data_type tt(0);
1804  find_segment(it, tt, t);
1805 
1806  if (it==segments.end())
1807  {
1808  assert(false);
1809  --it;
1810  }
1811 
1812  return it->second.tangent(tt);
1813  }
1814 
1815  void frenet_serret_frame(point_type &t, point_type &n, point_type &b, const data_type &t0)
1816  {
1817  // find segment that corresponds to given t
1818  typename segment_collection_type::const_iterator it;
1819  data_type tt(0);
1820  find_segment(it, tt, t0);
1821 
1822  if (it==segments.end())
1823  {
1824  assert(false);
1825  --it;
1826  }
1827 
1828  it->second.frenet_serret_frame(t, n, b, tt);
1829  }
1830 
1831  // TODO: NEED TO IMPLEMENT
1832  // * fit
1833  // * interpolate
1834 
1835  private:
1836  template<template<typename, unsigned short, typename> class curve1__,
1837  typename data1__, unsigned short dim1__, typename tol1__>
1841  template<template<typename, unsigned short, typename> class curve1__,
1842  typename data1__, unsigned short dim1__, typename tol1__>
1848 
1849  template<template<typename, unsigned short, typename> class curve1__, typename data1__, unsigned short dim1__, typename tol1__>
1855 
1856  typedef std::map<data_type, curve_type> segment_collection_type;
1857 
1858  segment_collection_type segments;
1859  data_type tmax;
1860  tolerance_type tol;
1861 
1862  private:
1863  template<typename it__>
1864  error_code split_seg(it__ it, const data_type &tt)
1865  {
1866  it__ itinsert;
1867  return split_seg(it, itinsert, tt);
1868  }
1869 
1870  template<typename it__>
1871  error_code split_seg(it__ it, it__ &itinsert, const data_type &tt)
1872  {
1873  // split the segment and replace
1874  data_type tr;
1875  curve_type cl, cr;
1876 
1877  it__ itnext = it;
1878  itnext++;
1879 
1880  it->second.split(cl, cr, tt);
1881  it->second = cl;
1882 
1883  data_type delta_t = get_delta_t(it);
1884 
1885  tr = it->first + delta_t*tt;
1886 
1887  itinsert = segments.insert(itnext, std::make_pair(tr, cr));
1888 
1890 
1891  return NO_ERRORS;
1892  }
1893 
1894  template<typename it__>
1895  void segment_to_cubic(it__ it, const data_type &ttol)
1896  {
1897  curve_type c = it->second;
1898  curve_type cc(c);
1899  cc.degree_to_cubic();
1900 
1901  data_type d = c.eqp_distance_bound(cc);
1902 
1903  if(d<ttol)
1904  {
1905  // replace c with cc.
1906  it->second = cc;
1907  }
1908  else
1909  {
1910  // split and recurse
1911  it__ itl;
1912  split_seg(it, itl, static_cast<data_type>(0.5));
1913 
1914  segment_to_cubic(itl, ttol);
1915  segment_to_cubic(it, ttol);
1916  }
1917  }
1918 
1920  {
1921  assert(!segments.empty());
1922 
1923  typename segment_collection_type::const_iterator it(segments.begin()), itp(it);
1924 
1925  for (++it; it!=segments.end(); ++it, ++itp)
1926  {
1927  data_type dt, dtp;
1928  dt = get_delta_t(it);
1929  dtp = get_delta_t(itp);
1930  if (!eli::geom::utility::check_point_continuity(itp->second, dtp, it->second, dt, cont, tol))
1931  {
1932  return false;
1933  }
1934  }
1935 
1936  return true;
1937  }
1938 
1939  data_type get_delta_t(const typename segment_collection_type::iterator &it) const
1940  {
1941  assert (it != segments.end());
1942 
1943  typename segment_collection_type::iterator itnext = it;
1944  itnext++;
1945 
1946  data_type delta_t;
1947 
1948  if(itnext != segments.end())
1949  delta_t = itnext->first - it->first;
1950  else
1951  delta_t = tmax - it->first;
1952 
1953  return delta_t;
1954  }
1955 
1956  data_type get_delta_t(const typename segment_collection_type::const_iterator &it) const
1957  {
1958  assert (it != segments.end());
1959 
1960  typename segment_collection_type::const_iterator itnext = it;
1961  itnext++;
1962 
1963  data_type delta_t;
1964 
1965  if(itnext != segments.end())
1966  delta_t = itnext->first - it->first;
1967  else
1968  delta_t = tmax - it->first;
1969 
1970  return delta_t;
1971  }
1972 
1973  data_type get_delta_t(const typename segment_collection_type::reverse_iterator &it) const
1974  {
1975  assert (it != segments.rend());
1976 
1977  data_type delta_t;
1978 
1979  if(it != segments.rbegin())
1980  {
1981  typename segment_collection_type::reverse_iterator itprev = it;
1982  itprev--;
1983  delta_t = itprev->first - it->first;
1984  }
1985  else
1986  {
1987  delta_t = tmax - it->first;
1988  }
1989 
1990  return delta_t;
1991  }
1992 
1993  data_type get_delta_t(const typename segment_collection_type::const_reverse_iterator &it) const
1994  {
1995  assert (it != segments.rend());
1996 
1997  data_type delta_t;
1998 
1999  if(it != segments.rbegin())
2000  {
2001  typename segment_collection_type::reverse_iterator itprev = it;
2002  itprev--;
2003  delta_t = itprev->first - it->first;
2004  }
2005  else
2006  {
2007  delta_t = tmax - it->first;
2008  }
2009 
2010  return delta_t;
2011  }
2012 
2013  void find_segment(typename segment_collection_type::const_iterator &it, const index_type &index) const
2014  {
2015  // advance to desired index
2016  index_type i;
2017  for (i=0, it=segments.begin(); i<index; ++i, ++it) {}
2018  }
2019 
2020  void find_segment(typename segment_collection_type::iterator &it, const index_type &index)
2021  {
2022  // advance to desired index
2023  index_type i;
2024  for (i=0, it=segments.begin(); i<index; ++i, ++it) {}
2025  }
2026 
2027  void find_segment(typename segment_collection_type::const_iterator &it, data_type &tt, const data_type &t_in) const
2028  {
2029  tol__ tol;
2030 
2031  // handle the end of the piecewise curve specially
2032  if(tol.approximately_equal(t_in, tmax))
2033  {
2034  tt=static_cast<data_type>(1);
2035  it=segments.end();
2036  it--;
2037  return;
2038  }
2039  if(t_in>tmax)
2040  {
2041  tt=static_cast<data_type>(2);
2042  it=segments.end();
2043  return;
2044  }
2045 
2046  // catch cases that are before the beginning of the piecewise curve
2047  if(t_in<get_parameter_min())
2048  {
2049  tt=static_cast<data_type>(-1);
2050  it=segments.end();
2051  return;
2052  }
2053 
2054  // Use map::upper_bound for fast lookup of segment after t_in
2055  it=segments.upper_bound(t_in);
2056 
2057  // Decrement to segment containing t_in
2058  if(it != segments.begin())
2059  {
2060  it--;
2061  }
2062 
2063  // At start of segment
2064  if(tol.approximately_equal(t_in, it->first))
2065  {
2066  tt=static_cast<data_type>(0);
2067  return;
2068  }
2069 
2070  data_type delta_t = get_delta_t(it);
2071 
2072  // At end of segment
2073  if(tol.approximately_equal(t_in, it->first + delta_t))
2074  {
2075  tt=static_cast<data_type>(1);
2076  return;
2077  }
2078 
2079  // Typical case
2080  tt=(t_in-it->first)/delta_t;
2081 
2082  // Super careful checks
2083  if (tt>static_cast<data_type>(1))
2084  {
2085  tt=static_cast<data_type>(1);
2086  }
2087  if (tt<static_cast<data_type>(0))
2088  {
2089  tt=static_cast<data_type>(0);
2090  }
2091  }
2092 
2093  void find_segment(typename segment_collection_type::iterator &it, data_type &tt, const data_type &t_in)
2094  {
2095  tol__ tol;
2096 
2097  // handle the end of the piecewise curve specially
2098  if(tol.approximately_equal(t_in, tmax))
2099  {
2100  tt=static_cast<data_type>(1);
2101  it=segments.end();
2102  it--;
2103  return;
2104  }
2105  if(t_in>tmax)
2106  {
2107  tt=static_cast<data_type>(2);
2108  it=segments.end();
2109  return;
2110  }
2111 
2112  // catch cases that are before the beginning of the piecewise curve
2113  if(t_in<get_parameter_min())
2114  {
2115  tt=static_cast<data_type>(-1);
2116  it=segments.end();
2117  return;
2118  }
2119 
2120  // Use map::upper_bound for fast lookup of segment after t_in
2121  it=segments.upper_bound(t_in);
2122 
2123  // Decrement to segment containing t_in
2124  if(it != segments.begin())
2125  it--;
2126 
2127  // At start of segment
2128  if(tol.approximately_equal(t_in, it->first))
2129  {
2130  tt=static_cast<data_type>(0);
2131  return;
2132  }
2133 
2134  data_type delta_t = get_delta_t(it);
2135 
2136  // At end of segment
2137  if(tol.approximately_equal(t_in, it->first + delta_t))
2138  {
2139  tt=static_cast<data_type>(1);
2140  return;
2141  }
2142 
2143  // Typical case
2144  tt=(t_in-it->first)/delta_t;
2145 
2146  // Super careful checks
2147  if (tt>static_cast<data_type>(1))
2148  {
2149  tt=static_cast<data_type>(1);
2150  }
2151  if (tt<static_cast<data_type>(0))
2152  {
2153  tt=static_cast<data_type>(0);
2154  }
2155 
2156  }
2157  };
2158  }
2159  }
2160 }
2161 #endif
curve_type::bounding_box_type bounding_box_type
Definition: piecewise.hpp:275
bool operator!=(const piecewise< curve__, data_type, dim__ > &p) const
Definition: piecewise.hpp:326
error_code degree_promote()
Definition: piecewise.hpp:735
Definition: continuity.hpp:26
void get_parameters(it__ itt) const
Definition: piecewise.hpp:380
Definition: math.hpp:20
void clear()
Definition: bounding_box.hpp:106
void split(bezier< data_type, dim__ > &bc_l, bezier< data_type, dim__ > &bc_r, const data_type &t0) const
Definition: bezier.hpp:502
Eigen::Matrix< data_type, dim__, dim__ > rotation_matrix_type
Definition: bezier.hpp:119
Definition: continuity.hpp:28
curve::piecewise< curve1__, data1__, dim1__, tol1__ >::data_type minimum_distance(typename curve::piecewise< curve1__, data1__, dim1__, tol1__ >::data_type &t, const curve::piecewise< curve1__, data1__, dim1__, tol1__ > &pc, const typename curve::piecewise< curve1__, data1__, dim1__, tol1__ >::point_type &pt)
Definition: bounding_box.hpp:27
error_code degree_promote_to(const index_type &index, const index_type &deg)
Definition: piecewise.hpp:769
data_type get_tmax() const
Definition: piecewise.hpp:333
Definition: continuity.hpp:34
void degrees(it__ itd)
Definition: piecewise.hpp:445
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: bezier.hpp:114
error_code split(piecewise< curve__, data_type, dim__ > &before, piecewise< curve__, data_type, dim__ > &after, const data_type &tsplit) const
Definition: piecewise.hpp:1074
void rotate(const rotation_matrix_type &rmat)
Definition: piecewise.hpp:471
Definition: piecewise.hpp:281
tol__ tolerance_type
Definition: piecewise.hpp:278
void find_discontinuities(const data_type &angle_tol, std::vector< data_type > &tdisc) const
Definition: piecewise.hpp:1704
point_type f(const data_type &t) const
Definition: bezier.hpp:324
error_code push_back(const curve_type &curve, const data_type &dt=1.0)
Definition: piecewise.hpp:688
data__ data_type
Definition: piecewise.hpp:276
error_code replace(const curve_type &curve, const index_type &index0, const index_type &index1)
Definition: piecewise.hpp:840
data_type tmax
Definition: piecewise.hpp:1859
eli::geom::general::continuity report_point_continuity(const curve1__ &curve1, const typename curve1__::data_type &dt1, const curve2__ &curve2, const typename curve2__::data_type &dt2, const tol__ &tol)
Definition: piecewise.hpp:226
Definition: continuity.hpp:32
data_type get_parameter_min() const
Definition: piecewise.hpp:366
piecewise(const piecewise< curve__, data_type, dim__, tol__ > &p)
Definition: piecewise.hpp:292
bool continuous(eli::geom::general::continuity cont, const data_type &t) const
Definition: piecewise.hpp:1415
curve_type::index_type index_type
Definition: piecewise.hpp:271
index_type number_segments() const
Definition: piecewise.hpp:419
void get_pmap(std::vector< data_type > &pmap)
Definition: piecewise.hpp:407
void reflect_xz()
Definition: piecewise.hpp:559
bool check_continuity(const eli::geom::general::continuity &cont) const
Definition: piecewise.hpp:1919
void rotate(const rotation_matrix_type &rmat, const point_type &rorig)
Definition: piecewise.hpp:481
piecewise()
Definition: piecewise.hpp:291
void find_segment(typename segment_collection_type::iterator &it, const index_type &index)
Definition: piecewise.hpp:2020
bool closed() const
Definition: piecewise.hpp:501
Definition: math.hpp:25
Definition: piecewise.hpp:287
Definition: piecewise.hpp:244
void segment_to_cubic(it__ it, const data_type &ttol)
Definition: piecewise.hpp:1895
void get_bounding_box(bounding_box_type &bb) const
Definition: piecewise.hpp:456
error_code push_front(const curve_type &curve, const data_type &dt=1.0)
Definition: piecewise.hpp:646
void frenet_serret_frame(point_type &t, point_type &n, point_type &b, const data_type &t0)
Definition: piecewise.hpp:1815
point_type tanget(const data_type &t) const
Definition: piecewise.hpp:1799
data_type get_delta_t(const typename segment_collection_type::reverse_iterator &it) const
Definition: piecewise.hpp:1973
void find_segment(typename segment_collection_type::const_iterator &it, data_type &tt, const data_type &t_in) const
Definition: piecewise.hpp:2027
eli::geom::general::continuity report_point_continuity(const curve1__ &curve1, const typename curve1__::data_type &dt1, const curve2__ &curve2, const typename curve2__::data_type &dt2, const eli::geom::general::continuity &cont, const tol__ &tol)
Definition: piecewise.hpp:124
point_type fpp(const data_type &t) const
Definition: piecewise.hpp:1765
tolerance_type tol
Definition: piecewise.hpp:1860
point_type fppp(const data_type &t) const
Definition: piecewise.hpp:1782
Definition: continuity.hpp:27
curve__< data__, dim__, tol__ > curve_type
Definition: piecewise.hpp:270
static dimension_type dimension()
Definition: piecewise.hpp:331
std::map< data_type, curve_type > segment_collection_type
Definition: piecewise.hpp:1856
Definition: tolerance.hpp:26
error_code degree_promote(const index_type &index)
Definition: piecewise.hpp:746
void translate(const point_type &trans)
Definition: piecewise.hpp:491
error_code replace(const piecewise< curve__, data_type, dim__ > &p, const index_type &index0, const index_type &index1)
Definition: piecewise.hpp:973
void degree_to_cubic()
Definition: bezier.hpp:494
Definition: continuity.hpp:33
Definition: continuity.hpp:30
error_code set(it__ itb, it__ ite, itd__ itd)
Definition: piecewise.hpp:624
bool add(const point_type &p)
Definition: bounding_box.hpp:113
error_code push_back(const piecewise< curve__, data_type, dim__, tol__ > &p)
Definition: piecewise.hpp:712
data_type get_delta_t(const typename segment_collection_type::const_reverse_iterator &it) const
Definition: piecewise.hpp:1993
piecewise & operator=(const piecewise< curve__, data_type, dim__ > &p)
Definition: piecewise.hpp:315
void length(typename piecewise< curve__, data__, dim__, tol__ >::data_type &len, const piecewise< curve__, data__, dim__, tol__ > &pc, const typename piecewise< curve__, data__, dim__, tol__ >::data_type &tol)
Definition: length.hpp:43
bool check_point_continuity(const curve1__ &curve1, const typename curve1__::data_type &dt1, const curve2__ &curve2, const typename curve2__::data_type &dt2, const eli::geom::general::continuity &cont, const tol__ &tol)
Definition: piecewise.hpp:33
~piecewise()
Definition: piecewise.hpp:293
void reflect(const point_type &normal)
Definition: piecewise.hpp:579
error_code set(it__ itb, it__ ite)
Definition: piecewise.hpp:602
error_code replace(const curve_type &curve, const index_type &index)
Definition: piecewise.hpp:796
error_code split_seg(it__ it, const data_type &tt)
Definition: piecewise.hpp:1864
Definition: continuity.hpp:35
void parameter_report() const
Definition: piecewise.hpp:391
curve_type::rotation_matrix_type rotation_matrix_type
Definition: piecewise.hpp:274
Definition: piecewise.hpp:282
error_code degree_promote_to(const index_type &deg)
Definition: piecewise.hpp:758
segment_collection_type segments
Definition: piecewise.hpp:1858
void find_segment(typename segment_collection_type::const_iterator &it, const index_type &index) const
Definition: piecewise.hpp:2013
void reverse()
Definition: piecewise.hpp:519
void clear()
Definition: piecewise.hpp:599
data_type get_parameter_max() const
Definition: piecewise.hpp:374
point_type fp(const data_type &t) const
Definition: piecewise.hpp:1748
friend void length(typename piecewise< curve1__, data1__, dim1__, tol1__ >::data_type &len, const piecewise< curve1__, data1__, dim1__, tol1__ > &pc, const typename piecewise< curve1__, data1__, dim1__, tol1__ >::data_type &tol)
point_type control_point_type
Definition: bezier.hpp:115
data_type get_t0() const
Definition: piecewise.hpp:335
void resize(const index_type &t_dim)
Definition: bezier.hpp:186
bool smooth(const data_type &angle_tol, const data_type &t) const
Definition: piecewise.hpp:1557
Definition: piecewise.hpp:284
void set_control_point(const control_point_type &cp, const index_type &i)
Definition: bezier.hpp:201
void round(const data_type &rad)
Definition: piecewise.hpp:1174
point_type f(const data_type &t) const
Definition: piecewise.hpp:1732
continuity
Definition: continuity.hpp:24
void set_t0(const data_type &t0_in)
Definition: piecewise.hpp:340
void degree(index_type &mind, index_type &maxd) const
Definition: piecewise.hpp:421
error_code split(const data_type &t)
Definition: piecewise.hpp:1055
point_type fp(const data_type &t) const
Definition: bezier.hpp:344
error_code push_front(const piecewise< curve__, data_type, dim__, tol__ > &p)
Definition: piecewise.hpp:671
void reflect_xy()
Definition: piecewise.hpp:549
data_type get_delta_t(const typename segment_collection_type::const_iterator &it) const
Definition: piecewise.hpp:1956
curve_type::control_point_type control_point_type
Definition: piecewise.hpp:273
Definition: continuity.hpp:36
void reflect_yz()
Definition: piecewise.hpp:569
void find_segment(typename segment_collection_type::iterator &it, data_type &tt, const data_type &t_in)
Definition: piecewise.hpp:2093
error_code replace(const piecewise< curve__, data_type, dim__ > &p, const index_type &index)
Definition: piecewise.hpp:892
void reflect(const point_type &normal, const data_type &d)
Definition: piecewise.hpp:589
curve_type::point_type point_type
Definition: piecewise.hpp:272
void find_discontinuities(eli::geom::general::continuity cont, std::vector< data_type > &tdisc) const
Definition: piecewise.hpp:1676
void to_cubic(const data_type &ttol)
Definition: piecewise.hpp:1167
bool round(const data_type &rad, const index_type &joint)
Definition: piecewise.hpp:1196
data_type eqp_distance_bound(const bezier< data_type, dim__, tolerance_type > &bc) const
Definition: bezier.hpp:174
bool open() const
Definition: piecewise.hpp:514
Definition: continuity.hpp:29
eli::geom::general::continuity continuity(const data_type &t) const
Definition: piecewise.hpp:1486
unsigned short dimension_type
Definition: piecewise.hpp:277
bool operator==(const piecewise< curve__, data_type, dim__ > &p) const
Definition: piecewise.hpp:295
error_code split_seg(it__ it, it__ &itinsert, const data_type &tt)
Definition: piecewise.hpp:1871
point_type::Index index_type
Definition: bezier.hpp:116
data_type get_delta_t(const typename segment_collection_type::iterator &it) const
Definition: piecewise.hpp:1939