Code-Eli  0.3.6
explicit_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_curve_explicit_bezier_hpp
14 #define eli_geom_curve_explicit_bezier_hpp
15 
16 #include "eli/code_eli.hpp"
17 
18 #include "eli/util/tolerance.hpp"
19 
24 
25 namespace eli
26 {
27  namespace geom
28  {
29  namespace curve
30  {
31  template<typename data__, typename tol__=eli::util::tolerance<data__> >
33  {
34  private:
36 
37  public:
39  typedef typename curve_type::data_type data_type;
40  typedef Eigen::Matrix<data_type, 1, 2> point_type;
45 
46  public:
48  explicit_bezier(const index_type &n) : y_curve(n) {}
51 
53  {
54  if (this==&eb)
55  return true;
56  return (y_curve==eb.y_curve);
57  }
58 
60  {
61  return !operator==(eb);
62  }
63 
64  static dimension_type dimension() {return 2;}
65 
66  void resize(const index_type &t_dim)
67  {
68  y_curve.resize(t_dim);
69  }
70 
71  bool open() const
72  {
73  return true;
74  }
75  bool closed() const
76  {
77  return false;
78  }
79 
80  index_type degree() const
81  {
82  return y_curve.degree();
83  }
84 
85  void set_control_point(const control_point_type &cp_in, const index_type &i)
86  {
87  y_curve.set_control_point(cp_in, i);
88  }
89 
90  control_point_type get_control_point(const index_type &i) const
91  {
92  return y_curve.get_control_point(i);
93  }
94 
95  void reverse()
96  {
97  y_curve.reverse();
98  }
99 
100  point_type f(const data_type &t) const
101  {
102  point_type rtn;
103  rtn << t, y_curve.f(t);
104  return rtn;
105  }
106 
107  point_type fp(const data_type &t) const
108  {
109  point_type rtn;
110  rtn << 1, y_curve.fp(t);
111  return rtn;
112  }
113 
114  point_type fpp(const data_type &t) const
115  {
116  point_type rtn;
117  rtn << 0, y_curve.fpp(t);
118  return rtn;
119  }
120 
121  point_type fppp(const data_type &t) const
122  {
123  point_type rtn;
124  rtn << 0, y_curve.fppp(t);
125  return rtn;
126  }
127 
128  point_type tangent(const data_type &t) const
129  {
130  point_type tgt(fp(t));
131 
132  tgt.normalize();
133  return tgt;
134  }
135 
136  void frenet_serret_frame(point_type &t, point_type &n, point_type &b, const data_type &t0)
137  {
138  t=tangent(t0);
139  n(0)=-t(1);
140  n(1)=t(0);
141  b.setZero();
142  }
143 
145  {
147  }
148 
150  {
151  return y_curve.degree_demote(continuity_degree);
152  }
153 
154  data_type fit(const fit_container_type &fcon, const index_type &deg_in)
155  {
156  std::vector<data_type> t;
157  return fit(t, fcon, deg_in);
158  }
159 
160  data_type fit(std::vector<data_type> &t, const fit_container_type &fcon, const index_type &deg_in)
161  {
162  size_t i, npts(fcon.number_points()), n;
163  std::vector<point_type, Eigen::aligned_allocator<point_type> > pts(npts);
164  std::vector<Eigen::Matrix<data_type, 1, 1>, Eigen::aligned_allocator<Eigen::Matrix<data_type, 1, 1> > > ypts(npts);
165 
166  // get the points from the container
167  fcon.get_points(pts.begin());
168 
169  // cannot have closed explicit curve
170  if (fcon.closed())
171  {
172  assert(false);
173  return -1;
174  }
175 
176  // the t values are the x-coordinates of fit points
177  t.resize(npts);
178  for (i=0; i<npts; ++i)
179  {
180  t[i]=pts[i](0);
181  ypts[i](0)=pts[i](1);
182  }
183 
184  // determine the actual degree of curve
185  n=internal::determine_n(deg_in, fcon.number_constraints(), npts);
186 
187  // build the fit terms from points
188  mat_type A, x;
189  row_pts_type b;
190  internal::build_fit_Ab(A, b, t, ypts, n, 1);
191 
192  // handle special case of unconstrained optimization problem
193  if (fcon.number_constraints()==0)
194  {
195  // determine the coefficients
197  }
198  else
199  {
200  // now becomes a constrained least squares problem
201  // the constraints come from closed flag and/or constraint collection
202  size_t bi, ncon=fcon.number_constraints();
203 
204  mat_type B(ncon, n+1);
205  row_pts_type d(ncon, 1);
206 
207  // construct the system of constraints
208  B.setZero();
209  d.setZero();
210 
211  size_t nconpts=fcon.number_constraint_points();
212  std::vector<typename fit_container_type::index_type> indexes(nconpts);
213 
214  // cycle through all of the constraints
215  fcon.get_constraint_indexes(indexes.begin());
216  bi=0;
217  for (size_t i=0; i<nconpts; ++i)
218  {
219  point_type pt;
221  typename fit_container_type::error_code ec;
222 
223  ec=fcon.get_constraint(indexes[i], ci);
225  {
226  assert(false);
227  }
228  else
229  {
230  // set the C0 constraint
231  col_type T;
232  mat_type N;
233  eli::geom::utility::bezier_T(T, t[indexes[i]], n);
235  B.row(bi)=T.transpose()*N;
236  d.row(bi)=ypts[indexes[i]];
237  ++bi;
238 
239  // set the C1 constraint
241  {
242  col_type Tp;
243 
244  eli::geom::utility::bezier_T_p(Tp, t[indexes[i]], n);
245  B.row(bi)=Tp.transpose()*N;
246  d.row(bi)=ci.get_fp();
247  ++bi;
248  }
249 
250  // set the C2 constraint
252  {
253  col_type Tpp;
254 
255  eli::geom::utility::bezier_T_pp(Tpp, t[indexes[i]], n);
256  B.row(bi)=Tpp.transpose()*N;
257  d.row(bi)=ci.get_fpp();
258  ++bi;
259  }
260  }
261  }
262 
263  // determine the coefficients
265  }
266 
267  // extract the control points and set them
268  resize(n);
269  for (i=0; i<=n; ++i)
270  {
271  set_control_point(x.row(i), i);
272  }
273 
274  // calculate the error at the point
275  data_type err(0);
276  for (i=0; i<pts.size(); ++i)
277  {
278  err+=eli::geom::point::distance(pts[i], f(t[i]));
279  }
280 
281  return err;
282  }
283 
284  void interpolate(const fit_container_type &fcon)
285  {
286  std::vector<data_type> t;
287  interpolate(t, fcon);
288  }
289 
290  void interpolate(std::vector<data_type> &t, const fit_container_type &fcon)
291  {
292  size_t i, npts(fcon.number_points()), n, ai;
293  std::vector<point_type, Eigen::aligned_allocator<point_type> > pts(npts);
294  std::vector<Eigen::Matrix<data_type, 1, 1>, Eigen::aligned_allocator<Eigen::Matrix<data_type, 1, 1> > > ypts(npts);
295 
296  // get the points from the container
297  fcon.get_points(pts.begin());
298 
299  // cannot have closed explicit curve
300  if (fcon.closed())
301  {
302  assert(false);
303  return;
304  }
305 
306  // the t values are the x-coordinates of fit points
307  t.resize(npts);
308  for (i=0; i<npts; ++i)
309  {
310  t[i]=pts[i](0);
311  ypts[i](0)=pts[i](1);
312  }
313 
314  // determine the actual degree of curve
315  n=fcon.number_constraints(false)+npts-1;
316 
317  // build the fit terms from points
318  mat_type A, x;
319  row_pts_type b;
320  internal::build_fit_Ab(A, b, t, ypts, n, 1);
321 
322  size_t nconpts=fcon.number_constraint_points();
323  std::vector<typename fit_container_type::index_type> indexes(nconpts);
324 
325  // cycle through all of the constraints
326  fcon.get_constraint_indexes(indexes.begin());
327  ai=pts.size();
328  for (size_t i=0; i<nconpts; ++i)
329  {
330  point_type pt;
332  typename fit_container_type::error_code ec;
333 
334  ec=fcon.get_constraint(indexes[i], ci);
336  {
337  assert(false);
338  }
339  else
340  {
341  // set the C1 constraint
343  {
344  col_type Tp;
345  mat_type N;
347 
348  eli::geom::utility::bezier_T_p(Tp, t[indexes[i]], n);
349  A.row(ai)=Tp.transpose()*N;
350  b.row(ai)=ci.get_fp();
351  ++ai;
352  }
353 
354  // set the C2 constraint
356  {
357  col_type Tpp;
358  mat_type N;
360 
361  eli::geom::utility::bezier_T_pp(Tpp, t[indexes[i]], n);
362  A.row(ai)=Tpp.transpose()*N;
363  b.row(ai)=ci.get_fpp();
364  ++ai;
365  }
366  }
367  }
368 
369  // solve for the control points
370  x=A.lu().solve(b);
371 
372  // extract the control points and set them
373  resize(n);
374  for (i=0; i<=n; ++i)
375  {
376  set_control_point(x.row(i), i);
377  }
378  }
379 
380  private:
381  typedef Eigen::Matrix<data_type, Eigen::Dynamic, 1> row_pts_type;
382  typedef Eigen::Matrix<data_type, Eigen::Dynamic, 1> col_type;
383  typedef Eigen::Matrix<data_type, Eigen::Dynamic, Eigen::Dynamic> mat_type;
384 
385  private:
386  curve_type y_curve;
387  };
388  }
389  }
390 }
391 #endif
void interpolate(std::vector< data_type > &t, const fit_container_type &fcon)
Definition: explicit_bezier.hpp:290
Definition: math.hpp:20
point_type f(const data_type &t) const
Definition: explicit_bezier.hpp:100
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
curve_type y_curve
Definition: explicit_bezier.hpp:386
curve_type::index_type index_type
Definition: explicit_bezier.hpp:42
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
geom::curve::fit_container< data_type, index_type, 2, 1 > fit_container_type
Definition: explicit_bezier.hpp:43
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: bezier.hpp:114
explicit_bezier(const explicit_bezier< data_type, tolerance_type > &eb)
Definition: explicit_bezier.hpp:49
curve_type::data_type data_type
Definition: explicit_bezier.hpp:39
void least_squares_uncon(Eigen::MatrixBase< data1__ > &x, const Eigen::MatrixBase< data2__ > &A, const Eigen::MatrixBase< data3__ > &r)
Definition: least_squares.hpp:25
point_type fpp(const data_type &t) const
Definition: explicit_bezier.hpp:114
explicit_bezier()
Definition: explicit_bezier.hpp:47
void degree_promote()
Definition: bezier.hpp:436
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, Eigen::Dynamic, 1 > col_type
Definition: explicit_bezier.hpp:382
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
bool open() const
Definition: explicit_bezier.hpp:71
Definition: explicit_bezier.hpp:32
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
use_states using_fpp() const
Definition: fit_container.hpp:89
data_type fit(const fit_container_type &fcon, const index_type &deg_in)
Definition: explicit_bezier.hpp:154
index_type degree() const
Definition: bezier.hpp:191
curve_type::tolerance_type tolerance_type
Definition: explicit_bezier.hpp:44
Definition: fit_container.hpp:43
void reverse()
Definition: explicit_bezier.hpp:95
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
curve_type::dimension_type dimension_type
Definition: explicit_bezier.hpp:38
Definition: continuity.hpp:27
void degree_promote()
Definition: explicit_bezier.hpp:144
bool degree_demote(const geom::general::continuity &continuity_degree=geom::general::C0)
Definition: explicit_bezier.hpp:149
bool closed() const
Definition: explicit_bezier.hpp:75
point_type get_fpp() const
Definition: fit_container.hpp:87
point_type tangent(const data_type &t) const
Definition: explicit_bezier.hpp:128
void resize(const index_type &t_dim)
Definition: explicit_bezier.hpp:66
index_type1 determine_n(const index_type1 &deg_in, const index_type2 &nconstrs, const index_type3 &npts)
Definition: bezier.hpp:78
curve_type::point_type control_point_type
Definition: explicit_bezier.hpp:41
data_type fit(std::vector< data_type > &t, const fit_container_type &fcon, const index_type &deg_in)
Definition: explicit_bezier.hpp:160
Eigen::Matrix< data_type, Eigen::Dynamic, 1 > row_pts_type
Definition: explicit_bezier.hpp:381
static dimension_type dimension()
Definition: explicit_bezier.hpp:64
index_type degree() const
Definition: explicit_bezier.hpp:80
explicit_bezier(const index_type &n)
Definition: explicit_bezier.hpp:48
control_point_type get_control_point(const index_type &i) const
Definition: explicit_bezier.hpp:90
void frenet_serret_frame(point_type &t, point_type &n, point_type &b, const data_type &t0)
Definition: explicit_bezier.hpp:136
point_type get_fp() const
Definition: fit_container.hpp:86
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
bool operator==(const explicit_bezier< data_type, tolerance_type > &eb) const
Definition: explicit_bezier.hpp:52
point_type fppp(const data_type &t) const
Definition: explicit_bezier.hpp:121
point_type fp(const data_type &t) const
Definition: explicit_bezier.hpp:107
~explicit_bezier()
Definition: explicit_bezier.hpp:50
continuity
Definition: continuity.hpp:24
bezier< data__, 1, tol__ > curve_type
Definition: explicit_bezier.hpp:35
error_code
Definition: fit_container.hpp:41
point_type fppp(const data_type &t) const
Definition: bezier.hpp:394
Eigen::Matrix< data_type, 1, 2 > point_type
Definition: explicit_bezier.hpp:40
void interpolate(const fit_container_type &fcon)
Definition: explicit_bezier.hpp:284
point_type fp(const data_type &t) const
Definition: bezier.hpp:344
void set_control_point(const control_point_type &cp_in, const index_type &i)
Definition: explicit_bezier.hpp:85
Definition: fit_container.hpp:50
data__ data_type
Definition: bezier.hpp:113
void get_points(it__ itb) const
Definition: fit_container.hpp:451
unsigned short dimension_type
Definition: bezier.hpp:112
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
bool closed() const
Definition: fit_container.hpp:396
bool operator!=(const explicit_bezier< data_type, tolerance_type > &eb) const
Definition: explicit_bezier.hpp:59
void bezier_T_pp(Eigen::MatrixBase< Derived > &Tpp, const typename Derived::Scalar &t, const typename Derived::Index &n)
Definition: bezier.hpp:374
Eigen::Matrix< data_type, Eigen::Dynamic, Eigen::Dynamic > mat_type
Definition: explicit_bezier.hpp:383
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