Code-Eli  0.3.6
bezier.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_utility_bezier_hpp
14 #define eli_geom_utility_bezier_hpp
15 
16 #include "eli/code_eli.hpp"
17 
19 
20 namespace eli
21 {
22  namespace geom
23  {
24  namespace utility
25  {
26  template<typename Derived1, typename Derived2>
27  void de_casteljau(Eigen::MatrixBase<Derived1> &p, const Eigen::MatrixBase<Derived2> &cp, const typename Derived2::Scalar &t)
28  {
29  // do some checks on incoming matrix dimensions
30  assert(p.cols()==cp.cols());
31 
32  Eigen::Matrix<typename Derived2::Scalar, Eigen::Dynamic, Eigen::Dynamic> Q(cp);
33  typename Derived2::Scalar one(1);
34  typename Derived2::Index i, k;
35 
36  for (k=1; k<Q.rows(); ++k)
37  {
38  for (i=0; i<Q.rows()-k; ++i)
39  {
40  Q.row(i)=(one-t)*Q.row(i)+t*Q.row(i+1);
41  }
42  }
43 
44  p=Q.row(0);
45  }
46 
47  template<typename Derived1, typename Derived2>
48  void bezier_p_control_point(Eigen::MatrixBase<Derived1> &cp_p, const Eigen::MatrixBase<Derived2> &cp)
49  {
50  // do some checks on incoming matrix dimensions
51  assert(cp_p.rows()==cp.rows()-1);
52  assert(cp_p.cols()==cp.cols());
53 
54  typename Derived2::Index i, n(cp.rows()-1);
55 
56  for (i=0; i<n; ++i)
57  {
58  cp_p.row(i)=static_cast<typename Derived2::Scalar>(n)*(cp.row(i+1)-cp.row(i));
59  }
60  }
61 
62  template<typename Derived1, typename Derived2>
63  void bezier_pp_control_point(Eigen::MatrixBase<Derived1> &cp_pp, const Eigen::MatrixBase<Derived2> &cp)
64  {
65  // do some checks on incoming matrix dimensions
66  assert(cp_pp.rows()==cp.rows()-2);
67  assert(cp_pp.cols()==cp.cols());
68 
69  typename Derived2::Index i, n(cp.rows()-1);
70 
71  for (i=0; i<n-1; ++i)
72  {
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));
74  }
75  }
76 
77  template<typename Derived1, typename Derived2>
78  void bezier_ppp_control_point(Eigen::MatrixBase<Derived1> &cp_ppp, const Eigen::MatrixBase<Derived2> &cp)
79  {
80  // do some checks on incoming matrix dimensions
81  assert(cp_ppp.rows()==cp.rows()-3);
82  assert(cp_ppp.cols()==cp.cols());
83 
84  typename Derived2::Index i, n(cp.rows()-1);
85 
86  for (i=0; i<n-2; ++i)
87  {
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));
89  }
90  }
91 
92  template<typename Derived1, typename Derived2>
93  void bezier_promote_control_points(Eigen::MatrixBase<Derived1> &cp_out, const Eigen::MatrixBase<Derived2> &cp_in)
94  {
95  // do some dimension checks
96  assert(cp_out.rows()==cp_in.rows()+1);
97  assert(cp_out.cols()==cp_in.cols());
98 
99  typename Derived1::Index i, n(cp_out.rows()-1);
100 
101  cp_out.row(0)=cp_in.row(0);
102  cp_out.row(n)=cp_in.row(n-1);
103  for (i=1; i<n; ++i)
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);
105  }
106 
107  template<typename Derived1, typename Derived2>
108  void bezier_promote_control_points_to(Eigen::MatrixBase<Derived1> &cp_out, const Eigen::MatrixBase<Derived2> &cp_in)
109  {
110  typedef typename Derived1::Index index_type;
111  typedef typename Derived1::Scalar data_type;
112 
113  // do some dimension checks
114  assert(cp_out.rows()>=cp_in.rows());
115  assert(cp_out.cols()==cp_in.cols());
116 
117  index_type i, ntarget(cp_out.rows()-1), nstart(cp_in.rows()-1);
118  index_type n(nstart);
119 
120  // Make in-place copy of control points.
121  for (i=0; i<n+1; ++i)
122  cp_out.row(i)=cp_in.row(i);
123 
124  for (; n<ntarget; n++)
125  {
126  // Assign final value, n'th value no longer needed and can be replaced.
127  cp_out.row(n+1)=cp_out.row(n);
128  // Work backwards, calculating in-place.
129  for (i=n; i>0; --i)
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);
131  }
132  }
133 
134  // Calculate cubic 'equivalent' to bezier curve. If input curve has degree less than cubic,
135  // the curve is promoted to an exactly equivalent cubic curve. If the input curve is cubic,
136  // the output curve is copied from the input. If the input curve is higher order, the output
137  // curve is a cubic bezier curve that will match the input curve in endpoint position and
138  // slopes. This approximation is used because it is very simple and fast. This curve may be
139  // a terrible approximation of the original curve, but through successive subdivision, a
140  // piecewise cubic approximation of any curve to any accuracy should be possible.
141  template<typename Derived1, typename Derived2>
142  void bezier_control_points_to_cubic(Eigen::MatrixBase<Derived1> &cp_out, const Eigen::MatrixBase<Derived2> &cp_in)
143  {
144  typedef typename Derived1::Index index_type;
145  typedef typename Derived1::Scalar data_type;
146 
147  // do some dimension checks
148  assert(cp_out.rows() == 4);
149  assert(cp_out.cols() == cp_in.cols());
150 
151  if(cp_in.rows() < 4) // Promote
152  {
153  bezier_promote_control_points_to(cp_out, cp_in);
154  }
155  else if(cp_in.rows() == 4) // Do nothing
156  {
157  index_type i;
158  for( i=0; i<4; ++i)
159  cp_out.row(i) = cp_in.row(i);
160  }
161  else // C1 Cubic demote
162  {
163  index_type n(cp_in.rows()-1);
164  data_type s = static_cast<data_type>(n)/static_cast<data_type>(3);
165 
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);
170  }
171  }
172 
173  // NOTE: Implements Eck's method: Matthias Eck. Least Squares Degree reduction of Bézier curves.
174  // Computer-Aided Design. Volume 27, No. 11. (1995), 845-851
175  // NOTE: This paper also provides an algorithm for creating a bezier spline order reduced
176  // curve given a bezier spline and a desired tolerance. It also provides a means of
177  // reducing more than one order at once.
178  template<typename Derived1, typename Derived2>
179  void bezier_demote_control_points(Eigen::MatrixBase<Derived1> &cp_out, const Eigen::MatrixBase<Derived2> &cp_in, int ncon)
180  {
181  typedef typename Derived1::Index index_type;
182  typedef typename Derived1::Scalar data_type;
183 
184  // do some dimension checks
185  assert(cp_out.rows()==cp_in.rows()-1);
186  assert(cp_out.cols()==cp_in.cols());
187 
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);
191 
192  B_I.row(0)=cp_in.row(0);
193  for (i=1; i<n; ++i)
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);
198 
199  // calculate the blending terms
200  if (ncon==0)
201  {
202  // no interpolation on end points
203  // note: this comes from "Degree Reduction of Bezier Curves" by Dave Morgan
204  data_type coef(1/static_cast<data_type>(1<<(2*n-1))), sum(1), tmp;
205  lambda(0)=coef*sum;
206  for (i=1; i<n; ++i)
207  {
208  eli::mutil::dm::n_choose_k(tmp, 2*n, 2*i);
209  sum+=tmp;
210  lambda(i)=coef*sum;
211  }
212  }
213  else
214  {
215  // interpolating end points up-to alpha order derivative
216  data_type tmp, coef, sum(0);
217  index_type alpha(ncon/2);
218  eli::mutil::dm::n_choose_k(tmp, 2*n, n+2*alpha);
219  coef=1/tmp;
220  lambda(0)=coef*sum;
221  for (i=1; i<n; ++i)
222  {
223  if ( (i<alpha) || (i+alpha>n) )
224  sum+=0;
225  else
226  {
227  data_type tmp1, tmp2;
228  eli::mutil::dm::n_choose_k(tmp1, n, i-alpha);
229  eli::mutil::dm::n_choose_k(tmp2, n, i+alpha);
230  sum+=tmp1*tmp2;
231  }
232  lambda(i)=coef*sum;
233  }
234  }
235 
236  // calculate new control points
237  for (i=0; i<n; ++i)
238  cp_out.row(i)=B_I.row(i)*(1-lambda(i))+B_II.row(i)*lambda(i);
239  }
240 
241  // Calculate upper bound of equiparametric distance between bezier curves.
242  // This routine efficiently calculates an upper bound of the distance between
243  // equal parameter points on two bezier curves. While not useful for
244  // calculating distances between arbitrary curves, this is just the ticket
245  // for judging the quality of one curve meant to approximate another.
246  // This algorithm requires that both curves have identical order, so the
247  // lower order curve is first promoted to match the higher order curve.
248  // Then, the control points of the curves are differenced, resulting in
249  // the control points of the curve that is the equiparametric difference
250  // between the curves. Since the control points provide a convex hull of
251  // a curve, they represent the worst-case difference between curves. The
252  // maximum distance between the origin and a control point of this curve
253  // is an upper bound of the equipmarametric distance between the input
254  // curves.
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)
257  {
258  typedef typename Derived1::Index index_type;
259  typedef typename Derived1::Scalar data_type;
260 
261  // dimension check
262  assert(cp_a.cols()==cp_b.cols());
263 
264  index_type na(cp_a.rows()-1), nb(cp_b.rows()-1);
265 
266  // make working copies
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);
269 
270  if(na == nb)
271  {
272  }
273  else if(na > nb)
274  {
275  cp_B=cp_A; // Copy to allocate proper size
276  bezier_promote_control_points_to(cp_B, cp_b); // promote b to B
277  }
278  else
279  {
280  cp_A=cp_B; // Copy to allocate proper size
281  bezier_promote_control_points_to(cp_A, cp_a); // promote a to A
282  }
283 
284  // calculate control point differences and track farthest from origin
285  maxd = (cp_B-cp_A).rowwise().norm().maxCoeff();
286  }
287 
288  // get the coefficients for the split curves by constructing a de Casteljau triangle scheme.
289  // The lower side is the t<t0 section, and the higher side is the t>t0 section.
290  template<typename Derived1, typename Derived2>
291  void bezier_split_control_points(Eigen::MatrixBase<Derived1> &cp_lo, Eigen::MatrixBase<Derived1> &cp_hi,
292  const Eigen::MatrixBase<Derived2> &cp_in, const typename Derived2::Scalar &t)
293  {
294  typename Derived2::Index i, j, n(cp_in.rows()-1);
295  Eigen::Matrix<typename Derived2::Scalar, Eigen::Dynamic, Eigen::Dynamic> tri(cp_in);
296 
297  // do some dimensions check
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());
302 
303  // set the control points using de Casteljau's algorithm
304  for (i=0; i<=n; ++i)
305  {
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)
309  {
310  tri.row(j)=tri.row(j+1)*t+tri.row(j)*(1-t);
311  }
312  }
313  }
314 
315  template<typename Derived>
316  void bezier_N(Eigen::MatrixBase<Derived> &N, const typename Derived::Index &n)
317  {
318  typename Derived::Index i,j;
319  typename Derived::Scalar bc1, bc2;
320 
321  // size N
322  N.derived().resize(n+1,n+1);
323  N.setZero();
324  for (i=0; i<=n; ++i)
325  {
326  for (j=0; j<=n; ++j)
327  {
328  if (i+j<=n)
329  {
330  eli::mutil::dm::n_choose_k(bc1, n, j);
331  eli::mutil::dm::n_choose_k(bc2, n-j, n-i-j);
332  N(i,j)=bc1*bc2;
333  if ((n-i-j)%2==1)
334  N(i,j)*=-1;
335  }
336  }
337  }
338  }
339 
340  template<typename Derived>
341  void bezier_T(Eigen::MatrixBase<Derived> &T, const typename Derived::Scalar &t, const typename Derived::Index &n)
342  {
343  // check to make sure have valid curve
344  assert(n>=0);
345 
346  typename Derived::Index j, jj;
347 
348  T.derived().resize(n+1);
349  T.fill(1);
350  for (j=0; j<=n-1; ++j)
351  for (jj=0; jj<j+1; ++jj)
352  T(jj)*=t;
353  }
354 
355  template<typename Derived>
356  void bezier_T_p(Eigen::MatrixBase<Derived> &Tp, const typename Derived::Scalar &t, const typename Derived::Index &n)
357  {
358  // check to make sure have valid curve
359  assert(n>=0);
360 
361  typename Derived::Index j, jj;
362 
363  Tp.derived().resize(n+1);
364  Tp.setZero();
365  for (j=0; j<=n-1; ++j)
366  Tp(j)=static_cast<typename Derived::Scalar>(n-j);
367 
368  for (j=0; j<=n-2; ++j)
369  for (jj=0; jj<j+1; ++jj)
370  Tp(jj)*=t;
371  }
372 
373  template<typename Derived>
374  void bezier_T_pp(Eigen::MatrixBase<Derived> &Tpp, const typename Derived::Scalar &t, const typename Derived::Index &n)
375  {
376  // check to make sure have valid curve
377  assert(n>=0);
378 
379  typename Derived::Index j, jj;
380 
381  Tpp.derived().resize(n+1);
382  Tpp.setZero();
383  for (j=0; j<=n-2; ++j)
384  Tpp(j)=static_cast<typename Derived::Scalar>(n-j)*(n-j-1);
385 
386  for (j=0; j<=n-3; ++j)
387  for (jj=0; jj<j+1; ++jj)
388  Tpp(jj)*=t;
389  }
390 
391  template<typename Derived>
392  void bezier_T_ppp(Eigen::MatrixBase<Derived> &Tppp, const typename Derived::Scalar &t, const typename Derived::Index &n)
393  {
394  // check to make sure have valid curve
395  assert(n>=0);
396 
397  typename Derived::Index j, jj;
398 
399  Tppp.derived().resize(n+1);
400  Tppp.setZero();
401  for (j=0; j<=n-3; ++j)
402  Tppp(j)=static_cast<typename Derived::Scalar>(n-j)*(n-j-1)*(n-j-2);
403 
404  for (j=0; j<=n-4; ++j)
405  for (jj=0; jj<j+1; ++jj)
406  Tppp(jj)*=t;
407  }
408 
409  }
410  }
411 }
412 
413 #endif
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
Definition: math.hpp:20
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