13 #ifndef eli_geom_utility_bezier_hpp
14 #define eli_geom_utility_bezier_hpp
16 #include "eli/code_eli.hpp"
26 template<
typename Derived1,
typename Derived2>
27 void de_casteljau(Eigen::MatrixBase<Derived1> &p,
const Eigen::MatrixBase<Derived2> &cp,
const typename Derived2::Scalar &t)
30 assert(p.cols()==cp.cols());
32 Eigen::Matrix<typename Derived2::Scalar, Eigen::Dynamic, Eigen::Dynamic> Q(cp);
33 typename Derived2::Scalar one(1);
34 typename Derived2::Index i, k;
36 for (k=1; k<Q.rows(); ++k)
38 for (i=0; i<Q.rows()-k; ++i)
40 Q.row(i)=(one-t)*Q.row(i)+t*Q.row(i+1);
47 template<
typename Derived1,
typename Derived2>
51 assert(cp_p.rows()==cp.rows()-1);
52 assert(cp_p.cols()==cp.cols());
54 typename Derived2::Index i, n(cp.rows()-1);
58 cp_p.row(i)=
static_cast<typename Derived2::Scalar
>(n)*(cp.row(i+1)-cp.row(i));
62 template<
typename Derived1,
typename Derived2>
66 assert(cp_pp.rows()==cp.rows()-2);
67 assert(cp_pp.cols()==cp.cols());
69 typename Derived2::Index i, n(cp.rows()-1);
73 cp_pp.row(i)=
static_cast<typename Derived2::Scalar
>(n)*(n-1)*(cp.row(i+2)-2*cp.row(i+1)+cp.row(i));
77 template<
typename Derived1,
typename Derived2>
81 assert(cp_ppp.rows()==cp.rows()-3);
82 assert(cp_ppp.cols()==cp.cols());
84 typename Derived2::Index i, n(cp.rows()-1);
88 cp_ppp.row(i)=
static_cast<typename Derived2::Scalar
>(n)*(n-1)*(n-2)*(cp.row(i+3)-3*cp.row(i+2)+3*cp.row(i+1)-cp.row(i));
92 template<
typename Derived1,
typename Derived2>
96 assert(cp_out.rows()==cp_in.rows()+1);
97 assert(cp_out.cols()==cp_in.cols());
99 typename Derived1::Index i, n(cp_out.rows()-1);
101 cp_out.row(0)=cp_in.row(0);
102 cp_out.row(n)=cp_in.row(n-1);
104 cp_out.row(i)=(cp_in.row(i-1)-cp_in.row(i))*(static_cast<typename Derived1::Scalar>(i)/n)+cp_in.row(i);
107 template<
typename Derived1,
typename Derived2>
110 typedef typename Derived1::Index index_type;
111 typedef typename Derived1::Scalar data_type;
114 assert(cp_out.rows()>=cp_in.rows());
115 assert(cp_out.cols()==cp_in.cols());
117 index_type i, ntarget(cp_out.rows()-1), nstart(cp_in.rows()-1);
118 index_type n(nstart);
121 for (i=0; i<n+1; ++i)
122 cp_out.row(i)=cp_in.row(i);
124 for (; n<ntarget; n++)
127 cp_out.row(n+1)=cp_out.row(n);
130 cp_out.row(i)=(cp_out.row(i-1)-cp_out.row(i))*(static_cast<data_type>(i)/(n+1))+cp_out.row(i);
141 template<
typename Derived1,
typename Derived2>
144 typedef typename Derived1::Index index_type;
145 typedef typename Derived1::Scalar data_type;
148 assert(cp_out.rows() == 4);
149 assert(cp_out.cols() == cp_in.cols());
155 else if(cp_in.rows() == 4)
159 cp_out.row(i) = cp_in.row(i);
163 index_type n(cp_in.rows()-1);
164 data_type s =
static_cast<data_type
>(n)/static_cast<data_type>(3);
166 cp_out.row(0) = cp_in.row(0);
167 cp_out.row(1) = cp_in.row(0) + s * (cp_in.row(1)-cp_in.row(0));
168 cp_out.row(2) = cp_in.row(n) + s * (cp_in.row(n-1)-cp_in.row(n));
169 cp_out.row(3) = cp_in.row(n);
178 template<
typename Derived1,
typename Derived2>
181 typedef typename Derived1::Index index_type;
182 typedef typename Derived1::Scalar data_type;
185 assert(cp_out.rows()==cp_in.rows()-1);
186 assert(cp_out.cols()==cp_in.cols());
188 index_type n(cp_in.rows()-1), i;
189 Eigen::Matrix<data_type, Eigen::Dynamic, Eigen::Dynamic> B_I(n, cp_in.cols()), B_II(n, cp_in.cols());
190 Eigen::Matrix<data_type, Eigen::Dynamic, 1> lambda(n);
192 B_I.row(0)=cp_in.row(0);
194 B_I.row(i)=(cp_in.row(i)*
static_cast<data_type
>(n)-B_I.row(i-1)*
static_cast<data_type
>(i))/
static_cast<data_type
>(n-i);
195 B_II.row(n-1)=cp_in.row(n);
196 for (i=n-1; i>=1; --i)
197 B_II.row(i-1)=(cp_in.row(i)*
static_cast<data_type
>(n)-B_II.row(i)*
static_cast<data_type
>(n-i))/
static_cast<data_type
>(i);
204 data_type coef(1/static_cast<data_type>(1<<(2*n-1))), sum(1), tmp;
216 data_type tmp, coef, sum(0);
217 index_type alpha(ncon/2);
223 if ( (i<alpha) || (i+alpha>n) )
227 data_type tmp1, tmp2;
238 cp_out.row(i)=B_I.row(i)*(1-lambda(i))+B_II.row(i)*lambda(i);
255 template<
typename Derived1,
typename Derived2>
256 void bezier_eqp_distance_bound(
const Eigen::MatrixBase<Derived1> &cp_a,
const Eigen::MatrixBase<Derived2> &cp_b,
typename Derived1::Scalar &maxd)
258 typedef typename Derived1::Index index_type;
259 typedef typename Derived1::Scalar data_type;
262 assert(cp_a.cols()==cp_b.cols());
264 index_type na(cp_a.rows()-1), nb(cp_b.rows()-1);
267 Eigen::Matrix<data_type, Eigen::Dynamic, Eigen::Dynamic> cp_A(cp_a);
268 Eigen::Matrix<data_type, Eigen::Dynamic, Eigen::Dynamic> cp_B(cp_b);
285 maxd = (cp_B-cp_A).rowwise().norm().maxCoeff();
290 template<
typename Derived1,
typename Derived2>
292 const Eigen::MatrixBase<Derived2> &cp_in,
const typename Derived2::Scalar &t)
294 typename Derived2::Index i, j, n(cp_in.rows()-1);
295 Eigen::Matrix<typename Derived2::Scalar, Eigen::Dynamic, Eigen::Dynamic> tri(cp_in);
298 assert(cp_lo.rows()==cp_hi.rows());
299 assert(cp_lo.cols()==cp_hi.cols());
300 assert(cp_lo.rows()==cp_in.rows());
301 assert(cp_lo.cols()==cp_in.cols());
306 cp_lo.row(i)=tri.row(0);
307 cp_hi.row(n-i)=tri.row(n-i);
308 for (j=0; j<n-i; ++j)
310 tri.row(j)=tri.row(j+1)*t+tri.row(j)*(1-t);
315 template<
typename Derived>
316 void bezier_N(Eigen::MatrixBase<Derived> &N,
const typename Derived::Index &n)
318 typename Derived::Index i,j;
319 typename Derived::Scalar bc1, bc2;
322 N.derived().resize(n+1,n+1);
340 template<
typename Derived>
341 void bezier_T(Eigen::MatrixBase<Derived> &T,
const typename Derived::Scalar &t,
const typename Derived::Index &n)
346 typename Derived::Index j, jj;
348 T.derived().resize(n+1);
350 for (j=0; j<=n-1; ++j)
351 for (jj=0; jj<j+1; ++jj)
355 template<
typename Derived>
356 void bezier_T_p(Eigen::MatrixBase<Derived> &Tp,
const typename Derived::Scalar &t,
const typename Derived::Index &n)
361 typename Derived::Index j, jj;
363 Tp.derived().resize(n+1);
365 for (j=0; j<=n-1; ++j)
366 Tp(j)=
static_cast<typename Derived::Scalar
>(n-j);
368 for (j=0; j<=n-2; ++j)
369 for (jj=0; jj<j+1; ++jj)
373 template<
typename Derived>
374 void bezier_T_pp(Eigen::MatrixBase<Derived> &Tpp,
const typename Derived::Scalar &t,
const typename Derived::Index &n)
379 typename Derived::Index j, jj;
381 Tpp.derived().resize(n+1);
383 for (j=0; j<=n-2; ++j)
384 Tpp(j)=
static_cast<typename Derived::Scalar
>(n-j)*(n-j-1);
386 for (j=0; j<=n-3; ++j)
387 for (jj=0; jj<j+1; ++jj)
391 template<
typename Derived>
392 void bezier_T_ppp(Eigen::MatrixBase<Derived> &Tppp,
const typename Derived::Scalar &t,
const typename Derived::Index &n)
397 typename Derived::Index j, jj;
399 Tppp.derived().resize(n+1);
401 for (j=0; j<=n-3; ++j)
402 Tppp(j)=
static_cast<typename Derived::Scalar
>(n-j)*(n-j-1)*(n-j-2);
404 for (j=0; j<=n-4; ++j)
405 for (jj=0; jj<j+1; ++jj)
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 bezier_T_ppp(Eigen::MatrixBase< Derived > &Tppp, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:392
void bezier_control_points_to_cubic(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in)
Definition: bezier.hpp:142
void bezier_ppp_control_point(Eigen::MatrixBase< Derived1 > &cp_ppp, const Eigen::MatrixBase< Derived2 > &cp)
Definition: bezier.hpp:78
void bezier_T_p(Eigen::MatrixBase< Derived > &Tp, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:356
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
void n_choose_k(data__ &v, natural__ n, natural__ k)
Definition: binomial_coefficient.hpp:25
void bezier_T(Eigen::MatrixBase< Derived > &T, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:341
void de_casteljau(Eigen::MatrixBase< Derived1 > &p, const Eigen::MatrixBase< Derived2 > &cp, const typename Derived2::Scalar &t)
Definition: bezier.hpp:27
void bezier_promote_control_points(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in)
Definition: bezier.hpp:93
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
void bezier_p_control_point(Eigen::MatrixBase< Derived1 > &cp_p, const Eigen::MatrixBase< Derived2 > &cp)
Definition: bezier.hpp:48
void bezier_promote_control_points_to(Eigen::MatrixBase< Derived1 > &cp_out, const Eigen::MatrixBase< Derived2 > &cp_in)
Definition: bezier.hpp:108
void bezier_N(Eigen::MatrixBase< Derived > &N, const typename Derived::Index &n)
Definition: bezier.hpp:316
void bezier_T_pp(Eigen::MatrixBase< Derived > &Tpp, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:374