13 #ifndef eli_geom_curve_bezier_h
14 #define eli_geom_curve_bezier_h
19 #include "eli/code_eli.hpp"
42 template<
typename Derived1,
typename Derived2,
typename Po
intType>
44 Eigen::MatrixBase<Derived2> &b,
45 std::vector<typename Derived1::Scalar> &t,
46 const std::vector<PointType, Eigen::aligned_allocator<PointType> > &pts,
47 const typename Derived1::Index &n,
const size_t &dim)
49 typedef Eigen::Matrix<typename Derived1::Scalar, Eigen::Dynamic, Eigen::Dynamic> mat_type;
50 typedef Eigen::Matrix<typename Derived1::Scalar, Eigen::Dynamic, 1> col_type;
52 typename Derived1::Index i, sz(n+1), npts(pts.size()), nrows(std::max(npts, sz));
53 mat_type N, T(nrows,sz);
56 A.derived().resize(nrows, sz);
57 b.derived().resize(nrows, dim);
61 for (i=0; i<npts; ++i)
65 T.row(i)=Tvec.transpose();
68 for (i=npts; i<nrows; ++i)
77 template<
typename index_type1,
typename index_type2,
typename index_type3>
78 index_type1
determine_n(
const index_type1 °_in,
const index_type2 &nconstrs,
const index_type3 &npts)
84 if (deg_in+1>static_cast<index_type1>(npts+nconstrs))
87 std::cerr <<
"deg_in (" << deg_in <<
") too low. Order should be " << n << std::endl;
108 template<
typename data__,
unsigned short dim__,
typename tol__=eli::util::tolerance<data__> >
124 bezier(
const index_type &n) :
B((n<=0)?(1):(n+1), dim__) {}
141 if ((
B.rows()!=
B.rows()) || (
B.cols()!=
B.cols()))
160 if ((
B.rows()!=
B.rows()) || (
B.cols()!=
B.cols()))
163 for (index_type i=0; i<=
degree(); ++i)
188 B.resize(t_dim+1, dim__);
242 point_type n(normal);
245 B=
B-2*(
B*n.transpose())*n;
248 void reflect(
const point_type &normal,
const data_type &d)
250 point_type n(normal);
253 B=
B-2*(
B*n.transpose()-d*Eigen::Matrix<data_type, Eigen::Dynamic, 1>::Ones(
degree()+1, 1))*n;
258 index_type i, n(
degree());
263 B_new.row(n-i)=
B.row(i);
272 index_type i, deg(
degree());
275 for (i=0; i<=deg; ++i)
281 void rotate(
const rotation_matrix_type &rmat)
286 void rotate(
const rotation_matrix_type &rmat,
const point_type &rorig)
295 index_type i, deg(
degree());
296 for (i=0; i<=deg; ++i)
304 index_type i, deg(
degree());
305 for (i=0; i<=deg; ++i)
316 for (index_type i=0; i<dim__; ++i)
318 if (!tol.approximately_equal(
B(0, i),
B(
degree(), i)))
324 point_type
f(
const data_type &t)
const
330 assert((t>=0) && (t<=1));
344 point_type
fp(
const data_type &t)
const
350 assert((t>=0) && (t<=1));
369 point_type
fpp(
const data_type &t)
const
375 assert((t>=0) && (t<=1));
394 point_type
fppp(
const data_type &t)
const
400 assert((t>=0) && (t<=1));
421 point_type tgt(
fp(t));
430 b=
fp(t0).cross(
fpp(t0));
431 n=-t.cross(b)/b.norm();
464 switch(continuity_degree)
504 if ( (t0>1) || (t0<0) )
520 for (index_type i=0; i<=n; ++i)
527 void fit(
const fit_container_type &fcon,
const index_type °_in)
529 std::vector<data_type> t;
533 void fit(std::vector<data_type> &t,
const fit_container_type &fcon,
const index_type °_in)
538 data_type
fit_with_error(
const fit_container_type &fcon,
const index_type °_in)
540 std::vector<data_type> t;
544 data_type
fit_with_error(std::vector<data_type> &t,
const fit_container_type &fcon,
const index_type °_in)
552 std::vector<data_type> t;
556 void interpolate(std::vector<data_type> &t,
const fit_container_type &fcon)
559 std::vector<point_type, Eigen::aligned_allocator<point_type> > pts(npts);
601 std::vector<typename fit_container_type::index_type> indexes(nconpts);
606 for (
size_t i=0; i<nconpts; ++i)
627 A.row(ai)=Tp.transpose()*N;
640 A.row(ai)=Tpp.transpose()*N;
687 for (i=0; i<n+1; ++i)
688 ctrl.row(i)=x.row(i);
692 ctrl.row(n)=ctrl.row(0);
700 typedef Eigen::Matrix<data_type, Eigen::Dynamic, 1>
col_type;
701 typedef Eigen::Matrix<data_type, 1, Eigen::Dynamic>
row_type;
702 typedef Eigen::Matrix<data_type, Eigen::Dynamic, Eigen::Dynamic>
mat_type;
705 control_point_matrix_type
B;
708 void determine_t(std::vector<data_type> &t,
const std::vector<point_type, Eigen::aligned_allocator<point_type> > &pts,
bool closed)
const
710 index_type i, npts(pts.size());
711 data_type len, small_dist(std::numeric_limits<data_type>::epsilon());
716 for (i=1; i<npts; ++i)
734 for (i=0; i<npts; ++i)
740 void fit_only(std::vector<data_type> &t,
const fit_container_type &fcon,
const index_type °_in)
743 std::vector<point_type, Eigen::aligned_allocator<point_type> > pts(npts);
796 mat_type
B(ncon, n+1);
797 row_pts_type d(ncon, dim__);
804 std::vector<typename fit_container_type::index_type> indexes(nconpts);
809 for (
size_t i=0; i<nconpts; ++i)
827 B.row(bi)=T.transpose()*N;
828 d.row(bi)=pts[indexes[i]];
837 B.row(bi)=Tp.transpose()*N;
848 B.row(bi)=Tpp.transpose()*N;
894 control_point_matrix_type ctrl(n+1, dim__);
895 for (i=0; i<n+1; ++i)
896 ctrl.row(i)=x.row(i);
900 ctrl.row(n)=ctrl.row(0);
905 data_type
est_fit_error(std::vector<data_type> &t,
const fit_container_type &fcon)
908 std::vector<point_type, Eigen::aligned_allocator<point_type> > pts(npts);
915 for (i=0; i<pts.size(); ++i)
Definition: continuity.hpp:26
void get_bounding_box(bounding_box_type &bb) const
Definition: bezier.hpp:270
void bezier_split_control_points(Eigen::MatrixBase< Derived1 > &cp_lo, Eigen::MatrixBase< Derived1 > &cp_hi, const Eigen::MatrixBase< Derived2 > &cp_in, const typename Derived2::Scalar &t)
Definition: bezier.hpp:291
void clear()
Definition: bounding_box.hpp:106
bool open() const
Definition: bezier.hpp:311
void reflect_xy()
Definition: bezier.hpp:225
bezier< float, 3 > bezier3f
Definition: bezier.hpp:927
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)
Derived1__::Scalar distance(const Eigen::MatrixBase< Derived1__ > &p1, const Eigen::MatrixBase< Derived2__ > &p2)
Definition: distance.hpp:33
use_states using_fp() const
Definition: fit_container.hpp:88
void interpolate(std::vector< data_type > &t, const fit_container_type &fcon)
Definition: bezier.hpp:556
void reflect_xz()
Definition: bezier.hpp:230
bezier< float, 2 > bezier2f
Definition: bezier.hpp:926
Definition: bounding_box.hpp:27
bool operator!=(const bezier< data_type, dim__, tolerance_type > &bc) const
Definition: bezier.hpp:148
void least_squares_eqcon(Eigen::MatrixBase< data1__ > &x, const Eigen::MatrixBase< data2__ > &A, const Eigen::MatrixBase< data3__ > &b, const Eigen::MatrixBase< data4__ > &B, const Eigen::MatrixBase< data5__ > &d)
Definition: least_squares.hpp:38
eli::geom::general::bounding_box< data_type, dim__, tolerance_type > bounding_box_type
Definition: bezier.hpp:120
void rotate(const rotation_matrix_type &rmat)
Definition: bezier.hpp:281
bezier()
Definition: bezier.hpp:123
void bezier_control_points_to_cubic(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in)
Definition: bezier.hpp:142
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: bezier.hpp:114
void bezier_ppp_control_point(Eigen::MatrixBase< Derived1 > &cp_ppp, const Eigen::MatrixBase< Derived2 > &cp)
Definition: bezier.hpp:78
void least_squares_uncon(Eigen::MatrixBase< data1__ > &x, const Eigen::MatrixBase< data2__ > &A, const Eigen::MatrixBase< data3__ > &r)
Definition: least_squares.hpp:25
void degree_promote()
Definition: bezier.hpp:436
void fit_only(std::vector< data_type > &t, const fit_container_type &fcon, const index_type °_in)
Definition: bezier.hpp:740
point_type f(const data_type &t) const
Definition: bezier.hpp:324
size_t number_constraint_points() const
Definition: fit_container.hpp:375
Eigen::Matrix< data_type, 1, Eigen::Dynamic > row_type
Definition: bezier.hpp:701
bezier< long double, 3 > bezier3ld
Definition: bezier.hpp:933
bezier & operator=(const bezier< data_type, dim__, tolerance_type > &bc)
Definition: bezier.hpp:128
bool closed() const
Definition: bezier.hpp:312
size_t number_points() const
Definition: fit_container.hpp:374
tol__ tolerance_type
Definition: bezier.hpp:118
void bezier_T_p(Eigen::MatrixBase< Derived > &Tp, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:356
Definition: fit_container.hpp:34
void bezier_eqp_distance_bound(const Eigen::MatrixBase< Derived1 > &cp_a, const Eigen::MatrixBase< Derived2 > &cp_b, typename Derived1::Scalar &maxd)
Definition: bezier.hpp:256
bezier< double, 1 > bezier1d
Definition: bezier.hpp:928
void degree_promote_to(const index_type target_degree)
Definition: bezier.hpp:448
void build_fit_Ab(Eigen::MatrixBase< Derived1 > &A, Eigen::MatrixBase< Derived2 > &b, std::vector< typename Derived1::Scalar > &t, const std::vector< PointType, Eigen::aligned_allocator< PointType > > &pts, const typename Derived1::Index &n, const size_t &dim)
Definition: bezier.hpp:43
void bezier_T(Eigen::MatrixBase< Derived > &T, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:341
bool approximately_equal(const bezier< data_type, dim__, tolerance_type > &bc) const
Definition: bezier.hpp:153
data_type fit_with_error(std::vector< data_type > &t, const fit_container_type &fcon, const index_type °_in)
Definition: bezier.hpp:544
use_states using_fpp() const
Definition: fit_container.hpp:89
Eigen::Matrix< data_type, Eigen::Dynamic, dim__ > control_point_matrix_type
Definition: bezier.hpp:698
void de_casteljau(Eigen::MatrixBase< Derived1 > &p, const Eigen::MatrixBase< Derived2 > &cp, const typename Derived2::Scalar &t)
Definition: bezier.hpp:27
point_type tangent(const data_type &t) const
Definition: bezier.hpp:419
data_type fit_with_error(const fit_container_type &fcon, const index_type °_in)
Definition: bezier.hpp:538
void translate(const point_type &trans)
Definition: bezier.hpp:293
void frenet_serret_frame(point_type &t, point_type &n, point_type &b, const data_type &t0)
Definition: bezier.hpp:427
data_type get_tmax() const
Definition: bezier.hpp:196
geom::curve::fit_container< data_type, index_type, dim__, dim__ > fit_container_type
Definition: bezier.hpp:117
control_point_matrix_type B
Definition: bezier.hpp:705
index_type degree() const
Definition: bezier.hpp:191
void clear()
Definition: bezier.hpp:184
Definition: fit_container.hpp:43
~bezier()
Definition: bezier.hpp:126
error_code get_constraint(const index_type &i, constraint_info &ci) const
Definition: fit_container.hpp:476
point_type fpp(const data_type &t) const
Definition: bezier.hpp:369
control_point_type get_control_point(const index_type &i) const
Definition: bezier.hpp:213
data_type est_fit_error(std::vector< data_type > &t, const fit_container_type &fcon)
Definition: bezier.hpp:905
Definition: continuity.hpp:27
point_type get_fpp() const
Definition: fit_container.hpp:87
const eli::geom::general::continuity & get_end_flag() const
Definition: fit_container.hpp:397
void degree_to_cubic()
Definition: bezier.hpp:494
void bezier_promote_control_points(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in)
Definition: bezier.hpp:93
index_type1 determine_n(const index_type1 °_in, const index_type2 &nconstrs, const index_type3 &npts)
Definition: bezier.hpp:78
bool add(const point_type &p)
Definition: bounding_box.hpp:113
data_type get_t0() const
Definition: bezier.hpp:197
void interpolate(const fit_container_type &fcon)
Definition: bezier.hpp:550
void rotate(const rotation_matrix_type &rmat, const point_type &rorig)
Definition: bezier.hpp:286
bezier< long double, 1 > bezier1ld
Definition: bezier.hpp:931
bool operator==(const bezier< data_type, dim__, tolerance_type > &bc) const
Definition: bezier.hpp:137
Definition: fit_container.hpp:57
void reflect(const point_type &normal, const data_type &d)
Definition: bezier.hpp:248
void fit(const fit_container_type &fcon, const index_type °_in)
Definition: bezier.hpp:527
void bezier_pp_control_point(Eigen::MatrixBase< Derived1 > &cp_pp, const Eigen::MatrixBase< Derived2 > &cp)
Definition: bezier.hpp:63
void bezier_demote_control_points(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in, int ncon)
Definition: bezier.hpp:179
point_type get_fp() const
Definition: fit_container.hpp:86
point_type control_point_type
Definition: bezier.hpp:115
void resize(const index_type &t_dim)
Definition: bezier.hpp:186
void set_control_point(const control_point_type &cp, const index_type &i)
Definition: bezier.hpp:201
void bezier_p_control_point(Eigen::MatrixBase< Derived1 > &cp_p, const Eigen::MatrixBase< Derived2 > &cp)
Definition: bezier.hpp:48
bezier< float, 1 > bezier1f
Definition: bezier.hpp:925
continuity
Definition: continuity.hpp:24
error_code
Definition: fit_container.hpp:41
bezier(const bezier< data_type, dim__, tolerance_type > &bc)
Definition: bezier.hpp:125
point_type fppp(const data_type &t) const
Definition: bezier.hpp:394
point_type fp(const data_type &t) const
Definition: bezier.hpp:344
bool open() const
Definition: fit_container.hpp:395
bezier< double, 2 > bezier2d
Definition: bezier.hpp:929
bezier< double, 3 > bezier3d
Definition: bezier.hpp:930
void determine_t(std::vector< data_type > &t, const std::vector< point_type, Eigen::aligned_allocator< point_type > > &pts, bool closed) const
Definition: bezier.hpp:708
Definition: fit_container.hpp:50
Eigen::Matrix< data_type, Eigen::Dynamic, 1 > col_type
Definition: bezier.hpp:700
data__ data_type
Definition: bezier.hpp:113
void get_points(it__ itb) const
Definition: fit_container.hpp:451
Eigen::Matrix< data_type, Eigen::Dynamic, dim__ > row_pts_type
Definition: bezier.hpp:699
void bezier_promote_control_points_to(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in)
Definition: bezier.hpp:108
Eigen::Matrix< data_type, Eigen::Dynamic, Eigen::Dynamic > mat_type
Definition: bezier.hpp:702
unsigned short dimension_type
Definition: bezier.hpp:112
Definition: bezier.hpp:109
bezier< long double, 2 > bezier2ld
Definition: bezier.hpp:932
void bezier_N(Eigen::MatrixBase< Derived > &N, const typename Derived::Index &n)
Definition: bezier.hpp:316
void reverse()
Definition: bezier.hpp:256
size_t number_constraints(bool fit=true) const
Definition: fit_container.hpp:377
error_code get_constraint_indexes(it__ itout) const
Definition: fit_container.hpp:688
data_type eqp_distance_bound(const bezier< data_type, dim__, tolerance_type > &bc) const
Definition: bezier.hpp:174
void scale(const data_type &s)
Definition: bezier.hpp:302
void fit(std::vector< data_type > &t, const fit_container_type &fcon, const index_type °_in)
Definition: bezier.hpp:533
bool closed() const
Definition: fit_container.hpp:396
bezier(const index_type &n)
Definition: bezier.hpp:124
Definition: continuity.hpp:29
void reflect_yz()
Definition: bezier.hpp:235
void bezier_T_pp(Eigen::MatrixBase< Derived > &Tpp, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:374
bool degree_demote(const geom::general::continuity &continuity_degree=geom::general::C0)
Definition: bezier.hpp:460
point_type::Index index_type
Definition: bezier.hpp:116
static dimension_type dimension()
Definition: bezier.hpp:199
void reflect(const point_type &normal)
Definition: bezier.hpp:240