Code-Eli  0.3.6
piecewise_connection_data.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2 * Copyright (c) 2014 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_surface_piecewise_connection_data_hpp
14 #define eli_geom_surface_piecewise_connection_data_hpp
15 
16 #include "eli/code_eli.hpp"
17 
18 #include "eli/util/tolerance.hpp"
19 
22 
23 #include <iterator>
24 
25 namespace eli
26 {
27  namespace geom
28  {
29  namespace surface
30  {
31  template<typename data__, unsigned short dim__, typename tol__>
33  {
34  public:
35  enum
36  {
37  CONNECTION_SET=0x000001, // must be set for valid joint
38  LEFT_FP_SET =0x000010,
39  RIGHT_FP_SET =0x000100,
40  LEFT_FPP_SET =0x001000,
41  RIGHT_FPP_SET =0x010000
42  };
43 
45  {
49  };
50 
51  typedef data__ data_type;
52  typedef tol__ tolerance_type;
55 
56  public:
58  {
59  }
60 
62  : f(cd.f), fp_left(cd.fp_left), fp_right(cd.fp_right), fpp_left(cd.fpp_left),
64  {
65  }
66 
68 
70  {
71  if (this!=&cd)
72  {
73  f=cd.f;
74  fp_left=cd.fp_left;
75  fp_right=cd.fp_right;
76  fpp_left=cd.fpp_left;
80  }
81 
82  return (*this);
83  }
84 
85  bool operator==(const connection_data &cd) const
86  {
87  tolerance_type tol;
88 
89  if (conditions!=cd.conditions)
90  return false;
91  if (continuity!=cd.continuity)
92  return false;
93  if (cd.use_f())
94  {
95  if (f!=cd.f)
96  return false;
97  }
98  if (cd.use_left_fp())
99  {
100  if (fp_left!=cd.fp_left)
101  return false;
102  }
103  if (cd.use_right_fp())
104  {
105  if (fp_right!=cd.fp_right)
106  return false;
107  }
108  if (cd.use_left_fpp())
109  {
110  if (fpp_left!=cd.fpp_left)
111  return false;
112  }
113  if (cd.use_right_fpp())
114  {
115  if (fpp_right!=cd.fpp_right)
116  return false;
117  }
118 
119  return true;
120  }
121 
122  bool operator!=(const connection_data &cd) const {return !operator==(cd);}
123 
124  data_type get_t0() const
125  {
126  return f.get_t0();
127  }
128  data_type get_tmax() const
129  {
130  return f.get_tmax();
131  }
132 
133  // get joints for all active curves
134  template<typename output_it__>
135  void get_joints(output_it__ it_in) const
136  {
137  tolerance_type tol;
138  auto comp = [&tol](const data_type &x1, const data_type &x2)->bool
139  {
140  return tol.approximately_less_than(x1, x2);
141  };
142 
143  std::vector<data_type> jts1, jts, jts_out;
144  if (use_f())
145  {
146  f.get_parameters(std::back_inserter(jts1));
147  if (jts.empty())
148  {
149  std::swap(jts, jts1);
150  }
151  else
152  {
153  std::set_union(jts.begin(), jts.end(), jts1.begin(), jts1.end(), std::back_inserter(jts_out), comp);
154  std::swap(jts, jts_out);
155  jts1.clear();
156  jts_out.clear();
157  }
158  }
159  if (use_left_fp())
160  {
161  fp_left.get_parameters(std::back_inserter(jts1));
162  if (jts.empty())
163  {
164  std::swap(jts, jts1);
165  }
166  else
167  {
168  std::set_union(jts.begin(), jts.end(), jts1.begin(), jts1.end(), std::back_inserter(jts_out), comp);
169  std::swap(jts, jts_out);
170  jts1.clear();
171  jts_out.clear();
172  }
173  }
174  if (use_right_fp())
175  {
176  fp_right.get_parameters(std::back_inserter(jts1));
177  if (jts.empty())
178  {
179  std::swap(jts, jts1);
180  }
181  else
182  {
183  std::set_union(jts.begin(), jts.end(), jts1.begin(), jts1.end(), std::back_inserter(jts_out), comp);
184  std::swap(jts, jts_out);
185  jts1.clear();
186  jts_out.clear();
187  }
188  }
189  if (use_left_fpp())
190  {
191  fpp_left.get_parameters(std::back_inserter(jts1));
192  if (jts.empty())
193  {
194  std::swap(jts, jts1);
195  }
196  else
197  {
198  std::set_union(jts.begin(), jts.end(), jts1.begin(), jts1.end(), std::back_inserter(jts_out), comp);
199  std::swap(jts, jts_out);
200  jts1.clear();
201  jts_out.clear();
202  }
203  }
204  if (use_right_fpp())
205  {
206  fpp_right.get_parameters(std::back_inserter(jts1));
207  if (jts.empty())
208  {
209  std::swap(jts, jts1);
210  }
211  else
212  {
213  std::set_union(jts.begin(), jts.end(), jts1.begin(), jts1.end(), std::back_inserter(jts_out), comp);
214  std::swap(jts, jts_out);
215  jts1.clear();
216  jts_out.clear();
217  }
218  }
219 
220  std::copy(jts.begin(), jts.end(), it_in);
221  }
222 
223  // split all curves
224  template<typename it1__, typename it2__>
225  void split(it1__ itb, it1__ ite, it2__ itd)
226  {
227  index_type i, njoints(0);
228  for (it1__ it=itb; it!=ite; ++it, ++njoints)
229  {
230  if (use_f())
231  {
232  f.split(*it);
233  }
234  if (use_left_fp())
235  {
236  fp_left.split(*it);
237  }
238  if (use_right_fp())
239  {
240  fp_right.split(*it);
241  }
242  if (use_left_fpp())
243  {
244  fpp_left.split(*it);
245  }
246  if (use_right_fpp())
247  {
248  fpp_right.split(*it);
249  }
250  }
251 
252  index_type nsegs(njoints-1);
253  std::vector<index_type> deg(nsegs), max_deg(nsegs, 0);
254  if (use_f())
255  {
256  f.degrees(deg.begin());
257  for (i=0; i<nsegs; ++i)
258  {
259  if (deg[i]>max_deg[i])
260  {
261  max_deg[i]=deg[i];
262  }
263  }
264  }
265  if (use_left_fp())
266  {
267  fp_left.degrees(deg.begin());
268  for (i=0; i<nsegs; ++i)
269  {
270  if (deg[i]>max_deg[i])
271  {
272  max_deg[i]=deg[i];
273  }
274  }
275  }
276  if (use_right_fp())
277  {
278  fp_right.degrees(deg.begin());
279  for (i=0; i<nsegs; ++i)
280  {
281  if (deg[i]>max_deg[i])
282  {
283  max_deg[i]=deg[i];
284  }
285  }
286  }
287  if (use_left_fpp())
288  {
289  fpp_left.degrees(deg.begin());
290  for (i=0; i<nsegs; ++i)
291  {
292  if (deg[i]>max_deg[i])
293  {
294  max_deg[i]=deg[i];
295  }
296  }
297  }
298  if (use_right_fpp())
299  {
300  fpp_right.degrees(deg.begin());
301  for (i=0; i<nsegs; ++i)
302  {
303  if (deg[i]>max_deg[i])
304  {
305  max_deg[i]=deg[i];
306  }
307  }
308  }
309 
310  for (i=0; i<nsegs; ++i, ++itd)
311  {
312  (*itd)=max_deg[i];
313  }
314  }
315 
316  template<typename it__>
317  bool promote(it__ itb, it__ ite)
318  {
319  // NOTE: the piecewise curve class will return error if degree vector is
320  // larger than number of segments
321  typename curve_type::error_code ec;
322  it__ it;
323  index_type i;
324  for (i=0, it=itb; it!=ite; ++it, ++i)
325  {
326  if (use_f())
327  {
328  ec=f.degree_promote_to(i, (*it));
329  if (ec!=curve_type::NO_ERRORS)
330  {
331  assert(false);
332  return false;
333  }
334  }
335  if (use_left_fp())
336  {
337  ec=fp_left.degree_promote_to(i, (*it));
338  if (ec!=curve_type::NO_ERRORS)
339  {
340  assert(false);
341  return false;
342  }
343  }
344  if (use_right_fp())
345  {
346  ec=fp_right.degree_promote_to(i, (*it));
347  if (ec!=curve_type::NO_ERRORS)
348  {
349  assert(false);
350  return false;
351  }
352  }
353  if (use_left_fpp())
354  {
355  ec=fpp_left.degree_promote_to(i, (*it));
356  if (ec!=curve_type::NO_ERRORS)
357  {
358  assert(false);
359  return false;
360  }
361  }
362  if (use_right_fpp())
363  {
364  ec=fpp_right.degree_promote_to(i, (*it));
365  if (ec!=curve_type::NO_ERRORS)
366  {
367  assert(false);
368  return false;
369  }
370  }
371  }
372 
373  return true;
374  }
375 
376  // connection interface
377  bool set_f(const curve_type &ff)
378  {
379  f=ff;
381  return check_state();
382  }
383  const curve_type & get_f() const {return f;}
384  bool unset_f()
385  {
387  return check_state();
388  }
389  bool use_f() const
390  {
391  return ((conditions & CONNECTION_SET) == CONNECTION_SET);
392  }
393 
394  // first derivative interface
395  bool set_left_fp(const curve_type &fpl)
396  {
397  fp_left=fpl;
399 
400  // if already wanting C1 continuous then set right as well
401  if (get_continuity()>C0)
402  {
403  fp_right=fpl;
405  }
406 
407  return check_state();
408  }
409  bool set_right_fp(const curve_type &fpr)
410  {
411  fp_right=fpr;
413 
414  // if already wanting C1 continuous then set left as well
415  if (get_continuity()>C0)
416  {
417  fp_left=fpr;
419  }
420 
421  return check_state();
422  }
423  bool set_fp(const curve_type &p)
424  {
426  fp_left=p;
427  fp_right=p;
428  return check_state();
429  }
430 
431  const curve_type & get_left_fp() const
432  {
433  return fp_left;
434  }
435  const curve_type & get_right_fp() const
436  {
437  return fp_right;
438  }
439  void get_fp(curve_type &fpl, curve_type &fpr) const
440  {
441  fpl=fp_left;
442  fpr=fp_right;
443  }
444 
445  bool unset_fp()
446  {
448  return check_state();
449  }
451  {
453 
454  return check_state();
455  }
457  {
459 
460  return check_state();
461  }
462 
463  bool use_left_fp() const
464  {
465  return ((conditions & LEFT_FP_SET) == LEFT_FP_SET);
466  }
467  bool use_right_fp() const
468  {
469  return ((conditions & RIGHT_FP_SET) == RIGHT_FP_SET);
470  }
471 
472  // second derivative interface
473  bool set_left_fpp(const curve_type &fppl)
474  {
475  fpp_left=fppl;
477 
478  // if already wanting C2 continuous then set right as well
479  if (get_continuity()>C1)
480  {
481  fpp_right=fppl;
483  }
484 
485  return check_state();
486  }
487  bool set_right_fpp(const curve_type &fppr)
488  {
489  fpp_right=fppr;
491 
492  // if already wanting C2 continuous then set left as well
493  if (get_continuity()>C1)
494  {
495  fpp_left=fppr;
497  }
498 
499  return check_state();
500  }
501  bool set_fpp(const curve_type &p)
502  {
504  fpp_left=p;
505  fpp_right=p;
506  return check_state();
507  }
508 
509  const curve_type & get_left_fpp() const
510  {
511  return fpp_left;
512  }
513  const curve_type & get_right_fpp() const
514  {
515  return fpp_right;
516  }
517  void get_fpp(curve_type &fppl, curve_type &fppr) const
518  {
519  fppl=fpp_left;
520  fppr=fpp_right;
521  }
522 
523  bool unset_fpp()
524  {
526  return check_state();
527  }
529  {
531 
532  return check_state();
533  }
535  {
537 
538  return check_state();
539  }
540 
541  bool use_left_fpp() const
542  {
543  return ((conditions & LEFT_FPP_SET) == LEFT_FPP_SET);
544  }
545  bool use_right_fpp() const
546  {
547  return ((conditions & RIGHT_FPP_SET) == RIGHT_FPP_SET);
548  }
549 
551  {
552  continuity=jc;
553  return check_state();
554  }
556  {
557  return continuity;
558  }
559 
560  bool check_state() const
561  {
562  tolerance_type tol;
563 
564  // check if point set
565  if ((conditions & CONNECTION_SET)==0)
566  return false;
567 
568  // if highest continuity is C0 then done
569  if (continuity==C0)
570  return true;
571 
572  // check first derivatives match on both sides
574  {
575  // TODO: make this comparison an apprimately_equal() call
576 // if (!tol.approximately_equal(fp_left, fp_right))
577  if (fp_left!=fp_right)
578  return false;
579  }
580  // check neither side first derivatives set set
581  else if ((conditions & (LEFT_FP_SET | RIGHT_FP_SET)) != 0)
582  {
583  return false;
584  }
585 
586  // check that the parameterization of fp is same as f
587  data_type t0(get_t0()), tmax(get_tmax());
588  if (use_left_fp())
589  {
590  if (!tol.approximately_equal(t0, fp_left.get_t0()) || !tol.approximately_equal(tmax, fp_left.get_tmax()))
591  return false;
592  }
593  if (use_right_fp())
594  {
595  if (!tol.approximately_equal(t0, fp_right.get_t0()) || !tol.approximately_equal(tmax, fp_right.get_tmax()))
596  return false;
597  }
598 
599  // if highest continuity is C1 then done
600  if (continuity==C1)
601  return true;
602 
603  // check second derivatives match on both sides
605  {
606  // TODO: make this comparison an apprimately_equal() call
607 // if (!tol.approximately_equal(fpp_left, fpp_right))
608  if (fpp_left!=fpp_right)
609  return false;
610  }
611  // check neither side second derivatives set set
612  else if ((conditions & (LEFT_FPP_SET | RIGHT_FPP_SET)) != 0)
613  {
614  return false;
615  }
616 
617  // check that the parameterization of fpp is same as f
618  if (use_left_fpp())
619  {
620  if (!tol.approximately_equal(t0, fpp_left.get_t0()) || !tol.approximately_equal(tmax, fpp_left.get_tmax()))
621  return false;
622  }
623  if (use_right_fpp())
624  {
625  if (!tol.approximately_equal(t0, fpp_right.get_t0()) || !tol.approximately_equal(tmax, fpp_right.get_tmax()))
626  return false;
627  }
628 
629  // since highest continuity can only be C2 then done
630  return true;
631  }
632 
633  private:
634  curve_type f;
635  curve_type fp_left, fp_right;
636  curve_type fpp_left, fpp_right;
637  unsigned int conditions;
639  };
640  }
641  }
642 }
643 #endif
bool set_right_fp(const curve_type &fpr)
Definition: piecewise_connection_data.hpp:409
bool operator!=(const connection_data &cd) const
Definition: piecewise_connection_data.hpp:122
bool set_fpp(const curve_type &p)
Definition: piecewise_connection_data.hpp:501
const curve_type & get_f() const
Definition: piecewise_connection_data.hpp:383
curve_type fp_left
Definition: piecewise_connection_data.hpp:635
bool unset_right_fp()
Definition: piecewise_connection_data.hpp:456
data_type get_t0() const
Definition: piecewise_connection_data.hpp:124
void get_parameters(it__ itt) const
Definition: piecewise.hpp:380
Definition: math.hpp:20
Definition: piecewise_connection_data.hpp:37
Definition: continuity.hpp:28
bool use_left_fpp() const
Definition: piecewise_connection_data.hpp:541
unsigned int conditions
Definition: piecewise_connection_data.hpp:637
bool check_state() const
Definition: piecewise_connection_data.hpp:560
data_type get_tmax() const
Definition: piecewise.hpp:333
void degrees(it__ itd)
Definition: piecewise.hpp:445
Definition: piecewise_connection_data.hpp:46
const curve_type & get_left_fpp() const
Definition: piecewise_connection_data.hpp:509
void get_fp(curve_type &fpl, curve_type &fpr) const
Definition: piecewise_connection_data.hpp:439
Definition: piecewise_connection_data.hpp:40
bool unset_fpp()
Definition: piecewise_connection_data.hpp:523
bool set_right_fpp(const curve_type &fppr)
Definition: piecewise_connection_data.hpp:487
curve_type fp_right
Definition: piecewise_connection_data.hpp:635
curve_type::index_type index_type
Definition: piecewise.hpp:271
connection_continuity get_continuity() const
Definition: piecewise_connection_data.hpp:555
const connection_data & operator=(const connection_data &cd)
Definition: piecewise_connection_data.hpp:69
bool set_continuity(connection_continuity jc)
Definition: piecewise_connection_data.hpp:550
const curve_type & get_right_fp() const
Definition: piecewise_connection_data.hpp:435
bool use_right_fp() const
Definition: piecewise_connection_data.hpp:467
tol__ tolerance_type
Definition: piecewise_connection_data.hpp:52
Definition: piecewise_connection_data.hpp:48
const curve_type & get_right_fpp() const
Definition: piecewise_connection_data.hpp:513
curve_type::index_type index_type
Definition: piecewise_connection_data.hpp:54
bool set_left_fpp(const curve_type &fppl)
Definition: piecewise_connection_data.hpp:473
curve_type fpp_left
Definition: piecewise_connection_data.hpp:636
Definition: continuity.hpp:27
data_type get_tmax() const
Definition: piecewise_connection_data.hpp:128
bool use_right_fpp() const
Definition: piecewise_connection_data.hpp:545
Definition: piecewise_connection_data.hpp:41
bool unset_right_fpp()
Definition: piecewise_connection_data.hpp:534
Definition: piecewise_connection_data.hpp:32
connection_continuity continuity
Definition: piecewise_connection_data.hpp:638
connection_continuity
Definition: piecewise_connection_data.hpp:44
const curve_type & get_left_fp() const
Definition: piecewise_connection_data.hpp:431
void split(it1__ itb, it1__ ite, it2__ itd)
Definition: piecewise_connection_data.hpp:225
connection_data()
Definition: piecewise_connection_data.hpp:57
data__ data_type
Definition: piecewise_connection_data.hpp:51
Definition: piecewise_connection_data.hpp:39
void get_joints(output_it__ it_in) const
Definition: piecewise_connection_data.hpp:135
bool operator==(const connection_data &cd) const
Definition: piecewise_connection_data.hpp:85
bool set_left_fp(const curve_type &fpl)
Definition: piecewise_connection_data.hpp:395
bool unset_f()
Definition: piecewise_connection_data.hpp:384
error_code degree_promote_to(const index_type &deg)
Definition: piecewise.hpp:758
bool set_fp(const curve_type &p)
Definition: piecewise_connection_data.hpp:423
Definition: piecewise_connection_data.hpp:38
curve_type fpp_right
Definition: piecewise_connection_data.hpp:636
bool use_left_fp() const
Definition: piecewise_connection_data.hpp:463
data_type get_t0() const
Definition: piecewise.hpp:335
bool use_f() const
Definition: piecewise_connection_data.hpp:389
bool promote(it__ itb, it__ ite)
Definition: piecewise_connection_data.hpp:317
~connection_data()
Definition: piecewise_connection_data.hpp:67
bool set_f(const curve_type &ff)
Definition: piecewise_connection_data.hpp:377
connection_data(const connection_data &cd)
Definition: piecewise_connection_data.hpp:61
curve_type f
Definition: piecewise_connection_data.hpp:634
Definition: piecewise_connection_data.hpp:47
error_code split(const data_type &t)
Definition: piecewise.hpp:1055
bool unset_left_fpp()
Definition: piecewise_connection_data.hpp:528
void get_fpp(curve_type &fppl, curve_type &fppr) const
Definition: piecewise_connection_data.hpp:517
bool unset_fp()
Definition: piecewise_connection_data.hpp:445
Definition: continuity.hpp:29
bool unset_left_fp()
Definition: piecewise_connection_data.hpp:450
eli::geom::curve::piecewise< eli::geom::curve::bezier, data__, dim__, tol__ > curve_type
Definition: piecewise_connection_data.hpp:53