Code-Eli  0.3.6
piecewise_general_creator.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_general_creator_hpp
14 #define eli_geom_curve_piecewise_general_creator_hpp
15 
16 #include <vector>
17 
18 #include "eli/code_eli.hpp"
19 
21 
23 
27 
28 namespace eli
29 {
30  namespace geom
31  {
32  namespace curve
33  {
34 
35  template<typename data__, unsigned short dim__, typename tol__>
36  class piecewise_general_creator : public piecewise_creator_base<data__, dim__, tol__>
37  {
38  public:
44 
45  enum
46  {
47  POINT_SET =0x000001, // must be set for valid joint
48  LEFT_FP_SET =0x000010,
49  RIGHT_FP_SET =0x000100,
50  LEFT_FPP_SET =0x001000,
51  RIGHT_FPP_SET=0x010000
52  };
53 
55  {
59  };
60 
61  class joint_data
62  {
63  public:
65  {
66  }
67 
69  : f(jd.f), fp_left(jd.fp_left), fp_right(jd.fp_right), fpp_left(jd.fpp_left),
71  {
72  }
73 
75 
76  const joint_data & operator=(const joint_data &jd)
77  {
78  if (this!=&jd)
79  {
80  f=jd.f;
81  fp_left=jd.fp_left;
82  fp_right=jd.fp_right;
83  fpp_left=jd.fpp_left;
87  }
88 
89  return (*this);
90  }
91 
92  bool operator==(const joint_data &jd) const
93  {
94  tolerance_type tol;
95 
96  if (conditions!=jd.conditions)
97  return false;
98  if (continuity!=jd.continuity)
99  return false;
100  if (jd.use_f())
101  {
102  if (!tol.approximately_equal(f, jd.f))
103  return false;
104  }
105  if (jd.use_left_fp())
106  {
107  if (!tol.approximately_equal(fp_left, jd.fp_left))
108  return false;
109  }
110  if (jd.use_right_fp())
111  {
112  if (!tol.approximately_equal(fp_right, jd.fp_right))
113  return false;
114  }
115  if (jd.use_left_fpp())
116  {
117  if (!tol.approximately_equal(fpp_left, jd.fpp_left))
118  return false;
119  }
120  if (jd.use_right_fpp())
121  {
122  if (!tol.approximately_equal(fpp_right, jd.fpp_right))
123  return false;
124  }
125 
126  return true;
127  }
128 
129  bool operator!=(const joint_data &jd) const {return !operator==(jd);}
130 
131  // point interface
132  bool set_f(const point_type &p)
133  {
134  f=p;
136  return check_state();
137  }
138  point_type get_f() const {return f;}
139  bool unset_f()
140  {
142  return check_state();
143  }
144  bool use_f() const
145  {
146  return ((conditions & POINT_SET) == POINT_SET);
147  }
148 
149  // first derivative interface
150  bool set_left_fp(const point_type &fpl)
151  {
152  fp_left=fpl;
154 
155  // if already wanting C1 continuous then set right as well
156  if (get_continuity()>C0)
157  {
158  fp_right=fpl;
160  }
161 
162  return check_state();
163  }
164  bool set_right_fp(const point_type &fpr)
165  {
166  fp_right=fpr;
168 
169  // if already wanting C1 continuous then set left as well
170  if (get_continuity()>C0)
171  {
172  fp_left=fpr;
174  }
175 
176  return check_state();
177  }
178  bool set_fp(const point_type &p)
179  {
181  fp_left=p;
182  fp_right=p;
183  return check_state();
184  }
185 
186  point_type get_left_fp() const
187  {
188  return fp_left;
189  }
190  point_type get_right_fp() const
191  {
192  return fp_right;
193  }
194  void get_fp(point_type &fpl, point_type &fpr) const
195  {
196  fpl=fp_left;
197  fpr=fp_right;
198  }
199 
200  bool unset_fp()
201  {
203  return check_state();
204  }
206  {
208 
209  return check_state();
210  }
212  {
214 
215  return check_state();
216  }
217 
218  bool use_left_fp() const
219  {
220  return ((conditions & LEFT_FP_SET) == LEFT_FP_SET);
221  }
222  bool use_right_fp() const
223  {
224  return ((conditions & RIGHT_FP_SET) == RIGHT_FP_SET);
225  }
226 
227  // second derivative interface
228  bool set_left_fpp(const point_type &fppl)
229  {
230  fpp_left=fppl;
232 
233  // if already wanting C2 continuous then set right as well
234  if (get_continuity()>C1)
235  {
236  fpp_right=fppl;
238  }
239 
240  return check_state();
241  }
242  bool set_right_fpp(const point_type &fppr)
243  {
244  fpp_right=fppr;
246 
247  // if already wanting C2 continuous then set left as well
248  if (get_continuity()>C1)
249  {
250  fpp_left=fppr;
252  }
253 
254  return check_state();
255  }
256  bool set_fpp(const point_type &p)
257  {
259  fpp_left=p;
260  fpp_right=p;
261  return check_state();
262  }
263 
264  point_type get_left_fpp() const
265  {
266  return fpp_left;
267  }
268  point_type get_right_fpp() const
269  {
270  return fpp_right;
271  }
272  void get_fpp(point_type &fppl, point_type &fppr) const
273  {
274  fppl=fpp_left;
275  fppr=fpp_right;
276  }
277 
278  bool unset_fpp()
279  {
281  return check_state();
282  }
284  {
286 
287  return check_state();
288  }
290  {
292 
293  return check_state();
294  }
295 
296  bool use_left_fpp() const
297  {
298  return ((conditions & LEFT_FPP_SET) == LEFT_FPP_SET);
299  }
300  bool use_right_fpp() const
301  {
302  return ((conditions & RIGHT_FPP_SET) == RIGHT_FPP_SET);
303  }
304 
306  {
307  continuity=jc;
308  return check_state();
309  }
311  {
312  return continuity;
313  }
314 
315  bool check_state() const
316  {
317  tolerance_type tol;
318 
319  // check if point set
320  if ((conditions & POINT_SET)==0)
321  return false;
322 
323  // if highest continuity is C0 then done
324  if (continuity==C0)
325  return true;
326 
327  // check first derivatives match on both sides
329  {
330  if (!tol.approximately_equal(fp_left, fp_right))
331  return false;
332  }
333  // check neither side first derivatives set set
334  else if ((conditions & (LEFT_FP_SET | RIGHT_FP_SET)) != 0)
335  {
336  return false;
337  }
338 
339  // if highest continuity is C1 then done
340  if (continuity==C1)
341  return true;
342 
343  // check second derivatives match on both sides
345  {
346  if (!tol.approximately_equal(fpp_left, fpp_right))
347  return false;
348  }
349  // check neither side second derivatives set set
350  else if ((conditions & (LEFT_FPP_SET | RIGHT_FPP_SET)) != 0)
351  {
352  return false;
353  }
354 
355  // since highest continuity can only be C2 then done
356  return true;
357  }
358 
359  private:
360  point_type f;
361  point_type fp_left, fp_right;
362  point_type fpp_left, fpp_right;
363  unsigned int conditions;
365  };
366 
367  public:
369  : piecewise_creator_base<data_type, dim__, tolerance_type>(1, 0), joints(2),
370  max_degree(1), closed(false)
371  {
372  }
373  piecewise_general_creator(const index_type &ns)
374  : piecewise_creator_base<data_type, dim__, tolerance_type>(ns, 0), joints(ns+1),
375  max_degree(ns), closed(false)
376  {
377  }
379  : piecewise_creator_base<data_type, dim__, tolerance_type>(pcc), joints(pcc.joints),
380  max_degree(pcc.max_degree()), closed(pcc.closed)
381  {
382  }
384  {
385  }
386 
387  void set_closed() {closed=true;}
388  void set_open() {closed=false;}
389  bool is_closed() const {return closed;}
390  bool is_open() const {return !closed;}
391 
392  bool set_conditions(const std::vector<joint_data> &jnts, const std::vector<index_type> &maxd, bool cl=false)
393  {
394  index_type i, nsegs(static_cast<index_type>(maxd.size())), njnts(jnts.size());
395 
396  // ensure input vectors are correct size
397  if (!cl && (njnts!=(nsegs+1)))
398  return false;
399  if (cl && (njnts!=nsegs))
400  return false;
401 
402  // check to make sure have valid end conditions
403  if (!cl)
404  {
405  if (jnts[0].use_left_fp() || jnts[0].use_left_fpp() || jnts[0].get_continuity()!=C0)
406  {
407  return false;
408  }
409  if (jnts[nsegs].use_right_fp() || jnts[nsegs].use_right_fpp() || jnts[nsegs].get_continuity()!=C0)
410  {
411  return false;
412  }
413  }
414 
415  // make sure joints are in valid state
416  for (i=0; i<njnts; ++i)
417  {
418  if (!jnts[i].check_state())
419  return false;
420  }
421 
422  // reset the number of segments
423  this->set_number_segments(nsegs);
424 
425  joints=jnts;
426  max_degree=maxd;
427  closed=cl;
428 
429  return true;
430  }
431 
433  {
434  index_type nsegs(this->get_number_segments()), i;
435  std::vector<index_type> seg_degree(nsegs);
436  std::vector<joint_data> joint_states(joints);
437 
438  pc.clear();
439 
440  // fix: cannot handle closed curves
441  assert(!closed);
442 
443  // cycle through each segment to find the minimum degree for each segment
444  for (i=0; i<nsegs; ++i)
445  {
446  seg_degree[i]=1;
447  if (joint_states[i].use_right_fp())
448  seg_degree[i]+=1;
449  if (joint_states[i+1].use_left_fp())
450  seg_degree[i]+=1;
451  if (joint_states[i].use_right_fpp())
452  seg_degree[i]+=1;
453  if (joint_states[i+1].use_left_fpp())
454  seg_degree[i]+=1;
455 
456  // check against maximum degree
457  if (!valid_degree(seg_degree[i], max_degree[i]))
458  {
459  return false;
460  }
461  }
462 
463  // cycle through each joint to find final degrees
464  for (i=0; i<=nsegs; ++i)
465  {
466  switch (joint_states[i].get_continuity())
467  {
468  // don't need to do anything for C0 case
469  case (C0):
470  {
471  break;
472  }
473  // increase degrees by at most two
474  case (C2):
475  // increase degrees by at most one
476  case (C1):
477  {
478  // handle C1 continuity
479  if (joint_states[i].use_left_fp())
480  {
481  // only need to increase degree if right is not in use
482  if (!joint_states[i].use_right_fp())
483  {
484  joint_states[i].set_right_fp(joint_states[i].get_left_fp());
485  // NOTE: Here is a place that needs to be modified if closed curve
486  seg_degree[i]+=1;
487  }
488  }
489  else
490  {
491  if (joint_states[i].use_right_fp())
492  {
493  joint_states[i].set_left_fp(joint_states[i].get_right_fp());
494  // NOTE: Here is a place that needs to be modified if closed curve
495  seg_degree[i-1]+=1;
496  }
497  else
498  {
499  bool hit_max_im1(max_degree[i-1]>0 && seg_degree[i-1]>=max_degree[i-1]);
500  bool hit_max_i(max_degree[i]>0 && seg_degree[i]>=max_degree[i]);
501 
502  // raise the degree of the lower degreed segment
503  if ((i==0) & (!closed))
504  {
505  seg_degree[i]+=1;
506  }
507  else if ((i==nsegs) && (!closed))
508  {
509  seg_degree[i-1]+=1;
510  }
511  // NOTE: Here is a place that needs to be modified if closed curve
512  else if ( hit_max_i || (!hit_max_im1 && (seg_degree[i-1]<=seg_degree[i])) )
513  {
514  seg_degree[i-1]+=1;
515  }
516  else
517  {
518  seg_degree[i]+=1;
519  }
520  }
521  }
522 
523  if (joint_states[i].get_continuity()==C1)
524  break;
525 
526  // handle C2 continuity
527  if (joint_states[i].use_left_fpp())
528  {
529  // only need to increase degree if right is not in use
530  if (!joint_states[i].use_right_fpp())
531  {
532  joint_states[i].set_right_fpp(joint_states[i].get_left_fpp());
533  // NOTE: Here is a place that needs to be modified if closed curve
534  seg_degree[i]+=1;
535  }
536  }
537  else
538  {
539  if (joint_states[i].use_right_fpp())
540  {
541  joint_states[i].set_left_fpp(joint_states[i].get_right_fpp());
542  // NOTE: Here is a place that needs to be modified if closed curve
543  seg_degree[i-1]+=1;
544  }
545  else
546  {
547  bool hit_max_im1(max_degree[i-1]>0 && seg_degree[i-1]>=max_degree[i-1]);
548  bool hit_max_i(max_degree[i]>0 && seg_degree[i]>=max_degree[i]);
549 
550  // raise the degree of the lower degreed segment
551  if ((i==0) && (!closed))
552  {
553  seg_degree[i]+=1;
554  }
555  else if ((i==nsegs) && (!closed))
556  {
557  seg_degree[i-1]+=1;
558  }
559  // NOTE: Here is a place that needs to be modified if closed curve
560  if ( hit_max_i || (!hit_max_im1 && (seg_degree[i-1]<=seg_degree[i])) )
561  {
562  seg_degree[i-1]+=1;
563 // assert(max_degree[i-1]<=0 || (seg_degree[i-1]<=max_degree[i-1]));
564  }
565  else
566  {
567  seg_degree[i]+=1;
568 // assert(max_degree[i]<=0 || (seg_degree[i]<=max_degree[i]));
569  }
570  }
571  }
572 
573  break;
574  }
575  // all cases should be handled by above
576  default:
577  {
578  assert(false);
579  break;
580  }
581  }
582  }
583 
584  // final check of maximum degree and determine number of unknowns
585  std::vector<index_type> seg_ind(nsegs+1);
586 
587  seg_ind[0]=0;
588  for (i=0; i<nsegs; ++i)
589  {
590  if (!valid_degree(seg_degree[i], max_degree[i]))
591  {
592  return false;
593  }
594 
595  seg_ind[i+1]=seg_ind[i]+seg_degree[i]+1;
596  }
597 
598  // build segments based on joint information
599  Eigen::Matrix<data_type, Eigen::Dynamic, Eigen::Dynamic> coef(seg_ind[nsegs]*dim__, seg_ind[nsegs]*dim__), rows(dim__, seg_ind[nsegs]*dim__);
600  Eigen::Matrix<data_type, Eigen::Dynamic, 1> x(seg_ind[nsegs]*dim__, 1), rhs(seg_ind[nsegs]*dim__, 1), rhs_seg(dim__, 1);
601  index_type cond_no(0);
602 
603  for (i=0; i<nsegs; ++i)
604  {
605  // set the end point conditions
606  assert(cond_no<coef.rows());
607  set_point_condition(rows, rhs_seg, seg_ind[i], seg_degree[i], joint_states[i].get_f(), true);
608  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
609  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
610  ++cond_no;
611 
612  assert(cond_no<coef.rows());
613  set_point_condition(rows, rhs_seg, seg_ind[i], seg_degree[i], joint_states[i+1].get_f(), false);
614  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
615  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
616  ++cond_no;
617 
618  // set the end point 1st derivative conditions
619  if (joint_states[i].use_right_fp())
620  {
621  assert(cond_no<coef.rows());
622  set_fp_condition(rows, rhs_seg, seg_ind[i], seg_degree[i], joint_states[i].get_right_fp(), this->get_segment_dt(i), true);
623  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
624  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
625  ++cond_no;
626  }
627 
628  if (joint_states[i+1].use_left_fp())
629  {
630  assert(cond_no<coef.rows());
631  set_fp_condition(rows, rhs_seg, seg_ind[i], seg_degree[i], joint_states[i+1].get_left_fp(), this->get_segment_dt(i), false);
632  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
633  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
634  ++cond_no;
635  }
636 
637  // set the end point 2nd derivative conditions
638  if (joint_states[i].use_right_fpp())
639  {
640  assert(cond_no<coef.rows());
641  set_fpp_condition(rows, rhs_seg, seg_ind[i], seg_degree[i], joint_states[i].get_right_fpp(), this->get_segment_dt(i), true);
642  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
643  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
644  ++cond_no;
645  }
646 
647  if (joint_states[i+1].use_left_fpp())
648  {
649  assert(cond_no<coef.rows());
650  set_fpp_condition(rows, rhs_seg, seg_ind[i], seg_degree[i], joint_states[i+1].get_left_fpp(), this->get_segment_dt(i), false);
651  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
652  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
653  ++cond_no;
654  }
655  }
656 
657  // cycle through each interior joint to set derivative continuity conditions
658  // FIX: This needs to be changed for closed curves
659  for (i=0; i<nsegs; ++i)
660  {
661  // set the 1st derivative continuous without specifying value
662  if ( (joint_states[i].get_continuity()>C0) && !joint_states[i].use_left_fp() && !joint_states[i].use_right_fp() )
663  {
664  assert(cond_no<coef.rows());
665  set_fp_continuous_condition(rows, rhs_seg, seg_ind[i-1], seg_degree[i-1], seg_degree[i], this->get_segment_dt(i-1), this->get_segment_dt(i));
666  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
667  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
668  ++cond_no;
669  }
670 
671  // set the 2nd derivative continuous without specifying value
672  if ( (joint_states[i].get_continuity()>C1) && !joint_states[i].use_left_fpp() && !joint_states[i].use_right_fpp() )
673  {
674  assert(cond_no<coef.rows());
675  set_fpp_continuous_condition(rows, rhs_seg, seg_ind[i-1], seg_degree[i-1], seg_degree[i], this->get_segment_dt(i-1), this->get_segment_dt(i));
676  coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
677  rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
678  ++cond_no;
679  }
680  }
681 
682  assert(cond_no*dim__==coef.rows());
683 
684  // solve for the control points
685  x=coef.lu().solve(rhs);
686 
687  // extract them into control points for each segment
688  typedef piecewise<bezier, data_type, dim__, tolerance_type> piecewise_curve_type;
689  typedef typename piecewise_curve_type::curve_type curve_type;
690  typedef typename piecewise_curve_type::error_code error_code;
691  typedef typename curve_type::control_point_type control_point_type;
692 
693  // set curve t0
694  pc.set_t0(this->get_t0());
695 
696  for (i=0; i<nsegs; ++i)
697  {
698  curve_type c(seg_degree[i]);
699  control_point_type cp;
700  error_code err;
701 
702  for (index_type j=0; j<=seg_degree[i]; ++j)
703  {
704  cp=x.block((seg_ind[i]+j)*dim__, 0, dim__, 1).transpose();
705  c.set_control_point(cp, j);
706  }
707 
708  err=pc.push_back(c, this->get_segment_dt(i));
709  if (err!=piecewise_curve_type::NO_ERRORS)
710  {
711  pc.clear();
712  pc.set_t0(0);
713  return false;
714  }
715  }
716 
717  return true;
718  }
719 
720  protected:
721  static bool valid_degree(const index_type &deg, const index_type &max_deg)
722  {
723  if (max_deg<=0)
724  return true;
725  if (deg<=max_deg)
726  return true;
727 
728  return false;
729  }
730 
731  template<typename Derived1, typename Derived2>
732  void set_point_condition(Eigen::MatrixBase<Derived1> &rows, Eigen::MatrixBase<Derived2> &rhs,
733  const index_type start_index, const index_type &seg_degree,
734  const point_type &p, bool segment_start) const
735  {
736  // set terms
737  index_type ind;
738 
739  if (segment_start)
740  {
741  ind=start_index*dim__;
742  }
743  else
744  {
745  ind=(start_index+seg_degree)*dim__;
746  }
747 
748  rows.setConstant(0);
749  rows.block(0, ind, dim__, dim__).setIdentity();
750  rhs=p.transpose();
751  }
752 
753  template<typename Derived1, typename Derived2>
754  void set_fp_condition(Eigen::MatrixBase<Derived1> &rows, Eigen::MatrixBase<Derived2> &rhs,
755  const index_type start_index, const index_type &seg_degree,
756  const point_type &fp, const data_type &dt, bool segment_start) const
757  {
758  assert(seg_degree>1);
759 
760  // set terms
761  index_type ind;
762  Eigen::Matrix<data_type, dim__, dim__> coef;
763 
764  // initialize rows & rhs to zero
765  rhs.setConstant(0);
766  rows.setConstant(0);
767 
768  if (segment_start)
769  {
770  ind=start_index*dim__;
771  }
772  else
773  {
774  ind=(start_index+seg_degree-1)*dim__;
775  }
776 
777  coef.setIdentity();
778  coef*=seg_degree/dt;
779  rows.setConstant(0);
780  rows.block(0, ind, dim__, dim__)=-coef;
781  rows.block(0, ind+dim__, dim__, dim__)=coef;
782  rhs=fp.transpose();
783  }
784 
785  template<typename Derived1, typename Derived2>
786  void set_fp_continuous_condition(Eigen::MatrixBase<Derived1> &rows, Eigen::MatrixBase<Derived2> &rhs,
787  const index_type start_index, const index_type &l_seg_degree,
788  const index_type &r_seg_degree, const data_type &l_dt,
789  const data_type &r_dt) const
790  {
791  assert( ((l_seg_degree>1) && (r_seg_degree>=1)) || ((r_seg_degree>1) && (l_seg_degree>=1)) );
792 
793  // set terms
794  index_type l_ind, r_ind;
795  Eigen::Matrix<data_type, dim__, dim__> coef;
796 
797  // initialize rows & rhs to zero
798  rhs.setConstant(0);
799  rows.setConstant(0);
800 
801  l_ind=(start_index+l_seg_degree-1)*dim__;
802  r_ind=(start_index+l_seg_degree+1)*dim__;
803 
804  coef.setIdentity();
805  coef*=l_seg_degree/l_dt;
806  rows.setConstant(0);
807  rows.block(0, l_ind, dim__, dim__)=-coef;
808  rows.block(0, l_ind+dim__, dim__, dim__)=coef;
809  coef.setIdentity();
810  coef*=r_seg_degree/r_dt;
811  rows.block(0, r_ind, dim__, dim__)=coef;
812  rows.block(0, r_ind+dim__, dim__, dim__)=-coef;
813  }
814 
815  template<typename Derived1, typename Derived2>
816  void set_fpp_condition(Eigen::MatrixBase<Derived1> &rows, Eigen::MatrixBase<Derived2> &rhs,
817  const index_type start_index, const index_type &seg_degree,
818  const point_type &fpp, const data_type &dt, bool segment_start) const
819  {
820  assert(seg_degree>1);
821 
822  // set terms
823  index_type ind;
824  Eigen::Matrix<data_type, dim__, dim__> coef;
825 
826  // initialize rows & rhs to zero
827  rhs.setConstant(0);
828  rows.setConstant(0);
829 
830  if (segment_start)
831  {
832  ind=start_index*dim__;
833  }
834  else
835  {
836  ind=(start_index+seg_degree-2)*dim__;
837  }
838 
839  coef.setIdentity();
840  coef*=seg_degree*(seg_degree-1)/dt/dt;
841  rows.setConstant(0);
842  rows.block(0, ind, dim__, dim__)=coef;
843  rows.block(0, ind+dim__, dim__, dim__)=-2*coef;
844  rows.block(0, ind+2*dim__, dim__, dim__)=coef;
845  rhs=fpp.transpose();
846  }
847 
848  template<typename Derived1, typename Derived2>
849  void set_fpp_continuous_condition(Eigen::MatrixBase<Derived1> &rows, Eigen::MatrixBase<Derived2> &rhs,
850  const index_type &start_index, const index_type &l_seg_degree,
851  const index_type &r_seg_degree, const data_type &l_dt,
852  const data_type &r_dt) const
853  {
854  assert( ((l_seg_degree>1) && (r_seg_degree>=1)) || ((r_seg_degree>1) && (l_seg_degree>=1)) );
855 
856  // set terms
857  index_type l_ind, r_ind;
858  Eigen::Matrix<data_type, dim__, dim__> coef;
859 
860  // initialize rows & rhs to zero
861  rhs.setConstant(0);
862  rows.setConstant(0);
863 
864  l_ind=(start_index+l_seg_degree-2)*dim__;
865  r_ind=(start_index+l_seg_degree+1)*dim__;
866 
867  // only set this if have enough control points to calculate the second derivative
868  if (l_seg_degree>1)
869  {
870  coef.setIdentity();
871  coef*=l_seg_degree*(l_seg_degree-1)/l_dt/l_dt;
872  rows.block(0, l_ind, dim__, dim__)=coef;
873  rows.block(0, l_ind+dim__, dim__, dim__)=-2*coef;
874  rows.block(0, l_ind+2*dim__, dim__, dim__)=coef;
875  }
876 
877  // only set this if have enough control points to calculate the second derivative
878  if (r_seg_degree>1)
879  {
880  coef.setIdentity();
881  coef*=r_seg_degree*(r_seg_degree-1)/r_dt/r_dt;
882  rows.block(0, r_ind, dim__, dim__)=-coef;
883  rows.block(0, r_ind+dim__, dim__, dim__)=2*coef;
884  rows.block(0, r_ind+2*dim__, dim__, dim__)=-coef;
885  }
886  }
887 
888  private:
889  std::vector<joint_data> joints;
890  std::vector<index_type> max_degree;
891  bool closed;
892  };
893  }
894  }
895 }
896 #endif
point_type f
Definition: piecewise_general_creator.hpp:360
point_type get_f() const
Definition: piecewise_general_creator.hpp:138
bool set_f(const point_type &p)
Definition: piecewise_general_creator.hpp:132
data__ data_type
Definition: piecewise_creator_base.hpp:33
Definition: math.hpp:20
virtual bool create(piecewise< bezier, data_type, dim__, tolerance_type > &pc) const
Definition: piecewise_general_creator.hpp:432
Definition: continuity.hpp:28
Definition: piecewise_general_creator.hpp:48
bool unset_left_fpp()
Definition: piecewise_general_creator.hpp:283
piecewise_general_creator()
Definition: piecewise_general_creator.hpp:368
bool unset_right_fpp()
Definition: piecewise_general_creator.hpp:289
bool set_right_fpp(const point_type &fppr)
Definition: piecewise_general_creator.hpp:242
bool set_continuity(joint_continuity jc)
Definition: piecewise_general_creator.hpp:305
piecewise_creator_base< data__, dim__, tol__ > base_class_type
Definition: piecewise_general_creator.hpp:39
bool unset_fpp()
Definition: piecewise_general_creator.hpp:278
void set_fpp_continuous_condition(Eigen::MatrixBase< Derived1 > &rows, Eigen::MatrixBase< Derived2 > &rhs, const index_type &start_index, const index_type &l_seg_degree, const index_type &r_seg_degree, const data_type &l_dt, const data_type &r_dt) const
Definition: piecewise_general_creator.hpp:849
bool set_conditions(const std::vector< joint_data > &jnts, const std::vector< index_type > &maxd, bool cl=false)
Definition: piecewise_general_creator.hpp:392
point_type get_right_fpp() const
Definition: piecewise_general_creator.hpp:268
bool set_right_fp(const point_type &fpr)
Definition: piecewise_general_creator.hpp:164
error_code push_back(const curve_type &curve, const data_type &dt=1.0)
Definition: piecewise.hpp:688
std::vector< index_type > max_degree
Definition: piecewise_general_creator.hpp:890
point_type get_right_fp() const
Definition: piecewise_general_creator.hpp:190
bool is_closed() const
Definition: piecewise_general_creator.hpp:389
bool unset_fp()
Definition: piecewise_general_creator.hpp:200
bool use_left_fp() const
Definition: piecewise_general_creator.hpp:218
point_type get_left_fpp() const
Definition: piecewise_general_creator.hpp:264
unsigned int conditions
Definition: piecewise_general_creator.hpp:363
~joint_data()
Definition: piecewise_general_creator.hpp:74
point_type fpp_right
Definition: piecewise_general_creator.hpp:362
bool check_state() const
Definition: piecewise_general_creator.hpp:315
bool operator!=(const joint_data &jd) const
Definition: piecewise_general_creator.hpp:129
bool set_fp(const point_type &p)
Definition: piecewise_general_creator.hpp:178
joint_continuity get_continuity() const
Definition: piecewise_general_creator.hpp:310
Definition: piecewise.hpp:244
point_type fp_left
Definition: piecewise_general_creator.hpp:361
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: piecewise_creator_base.hpp:34
bool unset_left_fp()
Definition: piecewise_general_creator.hpp:205
void set_fp_continuous_condition(Eigen::MatrixBase< Derived1 > &rows, Eigen::MatrixBase< Derived2 > &rhs, const index_type start_index, const index_type &l_seg_degree, const index_type &r_seg_degree, const data_type &l_dt, const data_type &r_dt) const
Definition: piecewise_general_creator.hpp:786
Definition: piecewise_general_creator.hpp:36
piecewise_general_creator(const index_type &ns)
Definition: piecewise_general_creator.hpp:373
bool use_right_fp() const
Definition: piecewise_general_creator.hpp:222
piecewise_general_creator(const piecewise_general_creator< data_type, dim__, tolerance_type > &pcc)
Definition: piecewise_general_creator.hpp:378
Definition: continuity.hpp:27
base_class_type::point_type point_type
Definition: piecewise_general_creator.hpp:41
index_type get_number_segments() const
Definition: piecewise_creator_base.hpp:47
base_class_type::tolerance_type tolerance_type
Definition: piecewise_general_creator.hpp:43
joint_data(const joint_data &jd)
Definition: piecewise_general_creator.hpp:68
bool operator==(const joint_data &jd) const
Definition: piecewise_general_creator.hpp:92
void set_fpp_condition(Eigen::MatrixBase< Derived1 > &rows, Eigen::MatrixBase< Derived2 > &rhs, const index_type start_index, const index_type &seg_degree, const point_type &fpp, const data_type &dt, bool segment_start) const
Definition: piecewise_general_creator.hpp:816
static bool valid_degree(const index_type &deg, const index_type &max_deg)
Definition: piecewise_general_creator.hpp:721
virtual ~piecewise_general_creator()
Definition: piecewise_general_creator.hpp:383
void get_fpp(point_type &fppl, point_type &fppr) const
Definition: piecewise_general_creator.hpp:272
const joint_data & operator=(const joint_data &jd)
Definition: piecewise_general_creator.hpp:76
void set_number_segments(const index_type &ns)
Definition: piecewise_creator_base.hpp:51
bool use_left_fpp() const
Definition: piecewise_general_creator.hpp:296
bool closed
Definition: piecewise_general_creator.hpp:891
Definition: piecewise_general_creator.hpp:51
Definition: piecewise_general_creator.hpp:57
data_type get_segment_dt(const index_type &i) const
Definition: piecewise_creator_base.hpp:78
void clear()
Definition: piecewise.hpp:599
point_type fp_right
Definition: piecewise_general_creator.hpp:361
joint_data()
Definition: piecewise_general_creator.hpp:64
bool unset_f()
Definition: piecewise_general_creator.hpp:139
void set_open()
Definition: piecewise_general_creator.hpp:388
bool unset_right_fp()
Definition: piecewise_general_creator.hpp:211
tol__ tolerance_type
Definition: piecewise_creator_base.hpp:36
Definition: piecewise_general_creator.hpp:58
Definition: piecewise_general_creator.hpp:47
void set_t0(const data_type &t0_in)
Definition: piecewise.hpp:340
void get_fp(point_type &fpl, point_type &fpr) const
Definition: piecewise_general_creator.hpp:194
bool set_fpp(const point_type &p)
Definition: piecewise_general_creator.hpp:256
bool use_right_fpp() const
Definition: piecewise_general_creator.hpp:300
void set_closed()
Definition: piecewise_general_creator.hpp:387
Definition: piecewise_general_creator.hpp:56
joint_continuity continuity
Definition: piecewise_general_creator.hpp:364
data_type get_t0() const
Definition: piecewise_creator_base.hpp:62
std::vector< joint_data > joints
Definition: piecewise_general_creator.hpp:889
point_type::Index index_type
Definition: piecewise_creator_base.hpp:35
base_class_type::index_type index_type
Definition: piecewise_general_creator.hpp:42
Definition: piecewise_general_creator.hpp:49
bool set_left_fpp(const point_type &fppl)
Definition: piecewise_general_creator.hpp:228
void set_point_condition(Eigen::MatrixBase< Derived1 > &rows, Eigen::MatrixBase< Derived2 > &rhs, const index_type start_index, const index_type &seg_degree, const point_type &p, bool segment_start) const
Definition: piecewise_general_creator.hpp:732
point_type get_left_fp() const
Definition: piecewise_general_creator.hpp:186
std::vector< data_type > dt
Definition: piecewise_creator_base.hpp:97
bool set_left_fp(const point_type &fpl)
Definition: piecewise_general_creator.hpp:150
bool use_f() const
Definition: piecewise_general_creator.hpp:144
point_type fpp_left
Definition: piecewise_general_creator.hpp:362
bool is_open() const
Definition: piecewise_general_creator.hpp:390
Definition: continuity.hpp:29
Definition: piecewise_creator_base.hpp:30
Definition: piecewise_general_creator.hpp:50
base_class_type::data_type data_type
Definition: piecewise_general_creator.hpp:40
void set_fp_condition(Eigen::MatrixBase< Derived1 > &rows, Eigen::MatrixBase< Derived2 > &rhs, const index_type start_index, const index_type &seg_degree, const point_type &fp, const data_type &dt, bool segment_start) const
Definition: piecewise_general_creator.hpp:754
joint_continuity
Definition: piecewise_general_creator.hpp:54
Definition: piecewise_general_creator.hpp:61