13 #ifndef eli_geom_curve_piecewise_general_creator_hpp
14 #define eli_geom_curve_piecewise_general_creator_hpp
18 #include "eli/code_eli.hpp"
35 template<
typename data__,
unsigned short dim__,
typename tol__>
102 if (!tol.approximately_equal(
f, jd.
f))
194 void get_fp(point_type &fpl, point_type &fpr)
const
272 void get_fpp(point_type &fppl, point_type &fppr)
const
392 bool set_conditions(
const std::vector<joint_data> &jnts,
const std::vector<index_type> &maxd,
bool cl=
false)
394 index_type i, nsegs(static_cast<index_type>(maxd.size())), njnts(jnts.size());
397 if (!cl && (njnts!=(nsegs+1)))
399 if (cl && (njnts!=nsegs))
405 if (jnts[0].use_left_fp() || jnts[0].use_left_fpp() || jnts[0].get_continuity()!=
C0)
409 if (jnts[nsegs].use_right_fp() || jnts[nsegs].use_right_fpp() || jnts[nsegs].get_continuity()!=
C0)
416 for (i=0; i<njnts; ++i)
418 if (!jnts[i].check_state())
435 std::vector<index_type> seg_degree(nsegs);
436 std::vector<joint_data> joint_states(
joints);
444 for (i=0; i<nsegs; ++i)
447 if (joint_states[i].use_right_fp())
449 if (joint_states[i+1].use_left_fp())
451 if (joint_states[i].use_right_fpp())
453 if (joint_states[i+1].use_left_fpp())
464 for (i=0; i<=nsegs; ++i)
466 switch (joint_states[i].get_continuity())
479 if (joint_states[i].use_left_fp())
482 if (!joint_states[i].use_right_fp())
484 joint_states[i].set_right_fp(joint_states[i].get_left_fp());
491 if (joint_states[i].use_right_fp())
493 joint_states[i].set_left_fp(joint_states[i].get_right_fp());
507 else if ((i==nsegs) && (!
closed))
512 else if ( hit_max_i || (!hit_max_im1 && (seg_degree[i-1]<=seg_degree[i])) )
523 if (joint_states[i].get_continuity()==
C1)
527 if (joint_states[i].use_left_fpp())
530 if (!joint_states[i].use_right_fpp())
532 joint_states[i].set_right_fpp(joint_states[i].get_left_fpp());
539 if (joint_states[i].use_right_fpp())
541 joint_states[i].set_left_fpp(joint_states[i].get_right_fpp());
555 else if ((i==nsegs) && (!
closed))
560 if ( hit_max_i || (!hit_max_im1 && (seg_degree[i-1]<=seg_degree[i])) )
585 std::vector<index_type> seg_ind(nsegs+1);
588 for (i=0; i<nsegs; ++i)
595 seg_ind[i+1]=seg_ind[i]+seg_degree[i]+1;
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);
603 for (i=0; i<nsegs; ++i)
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;
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;
619 if (joint_states[i].use_right_fp())
621 assert(cond_no<coef.rows());
623 coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
624 rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
628 if (joint_states[i+1].use_left_fp())
630 assert(cond_no<coef.rows());
632 coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
633 rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
638 if (joint_states[i].use_right_fpp())
640 assert(cond_no<coef.rows());
642 coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
643 rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
647 if (joint_states[i+1].use_left_fpp())
649 assert(cond_no<coef.rows());
651 coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
652 rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
659 for (i=0; i<nsegs; ++i)
662 if ( (joint_states[i].get_continuity()>
C0) && !joint_states[i].use_left_fp() && !joint_states[i].use_right_fp() )
664 assert(cond_no<coef.rows());
666 coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
667 rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
672 if ( (joint_states[i].get_continuity()>
C1) && !joint_states[i].use_left_fpp() && !joint_states[i].use_right_fpp() )
674 assert(cond_no<coef.rows());
676 coef.block(cond_no*dim__, 0, dim__, coef.cols())=rows;
677 rhs.block(cond_no*dim__, 0, dim__, 1)=rhs_seg;
682 assert(cond_no*dim__==coef.rows());
685 x=coef.lu().solve(rhs);
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;
696 for (i=0; i<nsegs; ++i)
698 curve_type c(seg_degree[i]);
699 control_point_type cp;
702 for (index_type j=0; j<=seg_degree[i]; ++j)
704 cp=x.block((seg_ind[i]+j)*dim__, 0, dim__, 1).transpose();
705 c.set_control_point(cp, j);
709 if (err!=piecewise_curve_type::NO_ERRORS)
721 static bool valid_degree(
const index_type °,
const index_type &max_deg)
731 template<
typename Derived1,
typename Derived2>
733 const index_type start_index,
const index_type &seg_degree,
734 const point_type &p,
bool segment_start)
const
741 ind=start_index*dim__;
745 ind=(start_index+seg_degree)*dim__;
749 rows.block(0, ind, dim__, dim__).setIdentity();
753 template<
typename Derived1,
typename Derived2>
755 const index_type start_index,
const index_type &seg_degree,
756 const point_type &fp,
const data_type &
dt,
bool segment_start)
const
758 assert(seg_degree>1);
762 Eigen::Matrix<data_type, dim__, dim__> coef;
770 ind=start_index*dim__;
774 ind=(start_index+seg_degree-1)*dim__;
780 rows.block(0, ind, dim__, dim__)=-coef;
781 rows.block(0, ind+dim__, dim__, dim__)=coef;
785 template<
typename Derived1,
typename Derived2>
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
791 assert( ((l_seg_degree>1) && (r_seg_degree>=1)) || ((r_seg_degree>1) && (l_seg_degree>=1)) );
794 index_type l_ind, r_ind;
795 Eigen::Matrix<data_type, dim__, dim__> coef;
801 l_ind=(start_index+l_seg_degree-1)*dim__;
802 r_ind=(start_index+l_seg_degree+1)*dim__;
805 coef*=l_seg_degree/l_dt;
807 rows.block(0, l_ind, dim__, dim__)=-coef;
808 rows.block(0, l_ind+dim__, dim__, dim__)=coef;
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;
815 template<
typename Derived1,
typename Derived2>
817 const index_type start_index,
const index_type &seg_degree,
818 const point_type &fpp,
const data_type &
dt,
bool segment_start)
const
820 assert(seg_degree>1);
824 Eigen::Matrix<data_type, dim__, dim__> coef;
832 ind=start_index*dim__;
836 ind=(start_index+seg_degree-2)*dim__;
840 coef*=seg_degree*(seg_degree-1)/dt/dt;
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;
848 template<
typename Derived1,
typename Derived2>
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
854 assert( ((l_seg_degree>1) && (r_seg_degree>=1)) || ((r_seg_degree>1) && (l_seg_degree>=1)) );
857 index_type l_ind, r_ind;
858 Eigen::Matrix<data_type, dim__, dim__> coef;
864 l_ind=(start_index+l_seg_degree-2)*dim__;
865 r_ind=(start_index+l_seg_degree+1)*dim__;
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;
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;
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
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 °, 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