Code-Eli  0.3.6
piecewise_circle_creator.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_piecewise_circle_creator_hpp
14 #define eli_geom_curve_piecewise_circle_creator_hpp
15 
16 #include <iterator>
17 
18 #include "eli/code_eli.hpp"
19 
20 #include "eli/constants/math.hpp"
21 
23 
27 
28 namespace eli
29 {
30  namespace geom
31  {
32  namespace curve
33  {
34  namespace utility
35  {
36  template<typename data__>
37  void calculate_circle(data__ &rad, data__ &x0, data__ &y0, const data__ &xa, const data__ &ya,
38  const data__ &xb, const data__ &yb, const data__ &xc, const data__ &yc)
39  {
40  data__ denom, xamb, yamb, xcma, ycma, xbmc, ybmc, alen2, blen2, clen2;
41 
42  xamb=(xa-xb);
43  yamb=(ya-yb);
44  xcma=(xc-xa);
45  ycma=(yc-ya);
46  xbmc=(xb-xc);
47  ybmc=(yb-yc);
48  alen2=xa*xa+ya*ya;
49  blen2=xb*xb+yb*yb;
50  clen2=xc*xc+yc*yc;
51 
52  denom=2*(xc*yamb+xb*ycma+xa*ybmc);
53  rad=std::sqrt((xamb*xamb+yamb*yamb)*(xcma*xcma+ycma*ycma)*(xbmc*xbmc+ybmc*ybmc))/denom;
54  x0= (clen2*yamb+blen2*ycma+alen2*ybmc)/denom;
55  y0=-(clen2*xamb+blen2*xcma+alen2*xbmc)/denom;
56  }
57  }
58 
59  template<typename data__, unsigned short dim__, typename tol__>
60  class piecewise_ellipse_creator_base : public piecewise_creator_base<data__, dim__, tol__>
61  {
62  public:
68 
69  public:
70  piecewise_ellipse_creator_base() : piecewise_creator_base<data_type, dim__, tolerance_type>(4, 0), xradius(1), yradius(1)
71  {
72  x.setZero(); x.x()=1;
73  y.setZero(); y.y()=1;
74  }
75  piecewise_ellipse_creator_base(const index_type &ns, const data_type &xr, const data_type &yr)
76  : piecewise_creator_base<data_type, dim__, tolerance_type>(ns, 0), xradius(xr), yradius(yr)
77  {
78  x.setZero(); x.x()=1;
79  y.setZero(); y.y()=1;
80  }
82  : piecewise_creator_base<data_type, dim__, tolerance_type>(pcc), xradius(pcc.xradius), yradius(pcc.yradius), x(pcc.x), y(pcc.y) {}
84 
85  void set_origin(const point_type &orig) {origin=orig;}
86  point_type get_origin() const {return origin;}
87 
88  void set_xy_directions(const point_type &xdir, const point_type &ydir)
89  {
90  tolerance_type tol;
91 
92  if (tol.approximately_equal(xdir.dot(ydir), 0))
93  {
94  x=xdir;
95  x.normalize();
96  y=ydir;
97  y.normalize();
98  }
99  else
100  {
101  assert(false);
102  }
103  }
104  void get_xy_directions(point_type &xdir, point_type &ydir) const
105  {
106  xdir=x;
107  ydir=y;
108  }
109 
111  {
112  typedef piecewise<bezier, data_type, dim__, tolerance_type> piecewise_curve_type;
113  typedef typename piecewise_curve_type::curve_type curve_type;
114  typedef typename piecewise_curve_type::error_code error_code;
115  typedef typename curve_type::control_point_type control_point_type;
116 
117  pc.clear();
118 
119  curve_type c(3);
120  control_point_type cp[4];
121  error_code err;
122  data_type k, xr, yr;
123  index_type i;
124 
125  // can only handle 4 segments for now
126  if (this->get_number_segments()!=4)
127  {
128  assert(false);
129  return false;
130  }
131 
132  // set the start parameter
133  pc.set_t0(this->get_t0());
134 
135  // set up for curve creation
136  pc.clear();
138  xr=get_x_radius();
139  yr=get_y_radius();
140 
141  // set 1st quadrant curve
142  cp[0]=xr*x+origin;
143  cp[1]=xr*x+yr*k*y+origin;
144  cp[2]=xr*k*x+yr*y+origin;
145  cp[3]=yr*y+origin;
146  for (i=0; i<4; ++i)
147  {
148  c.set_control_point(cp[i], i);
149  }
150  err=pc.push_back(c, this->get_segment_dt(0));
151  if (err!=piecewise_curve_type::NO_ERRORS)
152  {
153  pc.clear();
154  pc.set_t0(0);
155  return false;
156  }
157 
158  // set 2nd quadrant curve
159  cp[0]= yr*y+origin;
160  cp[1]= yr*y-xr*k*x+origin;
161  cp[2]= yr*k*y-xr*x+origin;
162  cp[3]=-xr*x+origin;
163  for (i=0; i<4; ++i)
164  {
165  c.set_control_point(cp[i], i);
166  }
167  err=pc.push_back(c, this->get_segment_dt(1));
168  if (err!=piecewise_curve_type::NO_ERRORS)
169  {
170  pc.clear();
171  pc.set_t0(0);
172  return false;
173  }
174 
175  // set 3rd quadrant curve
176  cp[0]=-xr*x+origin;
177  cp[1]=-xr*x-yr*k*y+origin;
178  cp[2]=-xr*k*x-yr*y+origin;
179  cp[3]=-yr*y+origin;
180  for (i=0; i<4; ++i)
181  {
182  c.set_control_point(cp[i], i);
183  }
184  err=pc.push_back(c, this->get_segment_dt(2));
185  if (err!=piecewise_curve_type::NO_ERRORS)
186  {
187  pc.clear();
188  pc.set_t0(0);
189  return false;
190  }
191 
192  // set 4th quadrant curve
193  cp[0]=-yr*y+origin;
194  cp[1]=-yr*y+xr*k*x+origin;
195  cp[2]=-yr*k*y+xr*x+origin;
196  cp[3]= xr*x+origin;
197  for (i=0; i<4; ++i)
198  {
199  c.set_control_point(cp[i], i);
200  }
201  err=pc.push_back(c, this->get_segment_dt(3));
202  if (err!=piecewise_curve_type::NO_ERRORS)
203  {
204  pc.clear();
205  pc.set_t0(0);
206  return false;
207  }
208 
209  return true;
210  }
211 
212  protected:
213  void set_x_radius(const data_type &xr)
214  {
215  if(xr>=0)
216  {
217  xradius=xr;
218  }
219  else
220  {
221  assert(false);
222  }
223  }
224  const data_type & get_x_radius() const {return xradius;}
225  void set_y_radius(const data_type &yr)
226  {
227  if (yr>=0)
228  {
229  yradius=yr;
230  }
231  else
232  {
233  assert(false);
234  }
235  }
236  const data_type & get_y_radius() const {return yradius;}
237 
238  private:
239  point_type origin, x, y;
240  data_type xradius, yradius;
241  };
242 
243  template<typename data__, unsigned short dim__, typename tol__>
244  class piecewise_circle_creator : public piecewise_ellipse_creator_base<data__, dim__, tol__>
245  {
246  public:
247  typedef data__ data_type;
248  typedef int index_type;
249  typedef Eigen::Matrix<data_type, 1, dim__> point_type;
250  typedef tol__ tolerance_type;
251 
252  public:
253  piecewise_circle_creator() : piecewise_ellipse_creator_base<data_type, dim__, tolerance_type>() {}
254  piecewise_circle_creator(const index_type &ns) : piecewise_ellipse_creator_base<data_type, dim__, tolerance_type>(ns, 1, 1) {}
256  : piecewise_ellipse_creator_base<data_type, dim__, tolerance_type>(pcc) {}
257 
258  void set_radius(const data_type &r)
259  {
260  tolerance_type tol;
261 
262  if (tol.approximately_equal(r, 0))
263  {
264  this->set_x_radius(0);
265  this->set_y_radius(0);
266  }
267  else if (r<0)
268  {
269  assert(false);
270  }
271  else
272  {
273  this->set_x_radius(r);
274  this->set_y_radius(r);
275  }
276  }
277  data_type get_radius() const
278  {
279  assert(this->get_x_radius()==this->get_y_radius());
280  return this->get_x_radius();
281  }
282 
283  void set(const point_type &orig, const point_type &x, const point_type &y, const data_type &r)
284  {
285  this->set_origin(orig);
286  this->set_xy_directions(x, y);
287  set_radius(r);
288  }
289 
290  void set(const point_type &start, const point_type &orig)
291  {
292  // need to be on the same z-plane
293  if (dim__!=2)
294  {
295  if (start.col(2)!=orig.col(2))
296  {
297  assert(false);
298  return;
299  }
300  }
301 
302  // set radius & origin
303  point_type x, y;
305  this->set_origin(orig);
306 
307  if (get_radius()==0)
308  {
309  x.setZero();
310  x(0)=1;
311  y.setZero();
312  y(1)=1;
313  }
314  else
315  {
316  x=start-this->get_origin();
317  x.normalize();
318  y.x()=-x.y();
319  y.y()=x.x();
320  if (dim__>2)
321  {
322  y.z()=0;
323  }
324  }
325  this->set_xy_directions(x, y);
326  }
327 
328  void set(const point_type &start, const point_type &orig, const point_type &normal)
329  {
330  if (dim__==2)
331  {
332  set(start, orig);
333  return;
334  }
335 
336  // set radius & origin
337  point_type x, y;
339  this->set_origin(orig);
340 
341  if (get_radius()==0)
342  {
343  x.setZero();
344  x(0)=1;
345  y.setZero();
346  y(1)=1;
347  }
348  else
349  {
350  x=start-this->get_origin();
351  x.normalize();
352  y << normal(1)*x(2)-normal(2)*x(1),
353  normal(2)*x(0)-normal(0)*x(2),
354  normal(0)*x(1)-normal(1)*x(0);
355  y.normalize();
356 
357 #ifdef DEBUG
358  point_type n(normal);
359  tolerance_type tol;
360  n.normalize();
361  assert(tol.approximately_equal(x.dot(n), 0));
362 #endif
363  }
364  this->set_xy_directions(x, y);
365  }
366 
367  void set_3pt(const point_type &start, const point_type &middle, const point_type &end)
368  {
369  tolerance_type tol;
370 
371  // cannot have points in same location
372  if ( tol.approximately_equal(start, end)
373  || tol.approximately_equal(start, middle)
374  || tol.approximately_equal(middle, end) )
375  {
376  assert(false);
377  return;
378  }
379 
380  if (dim__==2)
381  {
382  point_type orig;
383  data_type r;
384 
385  eli::geom::curve::utility::calculate_circle(r, orig.x(), orig.y(), start.x(), start.y(),
386  middle.x(), middle.y(), end.x(), end.y());
387  set(start, orig);
388  }
389  else
390  {
391  point_type orig, normal, xtmp, ytmp;
392 
393  // set the temporary x-direction as from start to middle
394  xtmp=middle-start;
395  xtmp.normalize();
396 
397  // find normal
398  ytmp=end-start;
399  normal << xtmp(1)*ytmp(2)-xtmp(2)*ytmp(1),
400  xtmp(2)*ytmp(0)-xtmp(0)*ytmp(2),
401  xtmp(0)*ytmp(1)-xtmp(1)*ytmp(0);
402  normal.normalize();
403 
404  // get the temporary y-direction
405  ytmp << normal(1)*xtmp(2)-normal(2)*xtmp(1),
406  normal(2)*xtmp(0)-normal(0)*xtmp(2),
407  normal(0)*xtmp(1)-normal(1)*xtmp(0);
408  ytmp.normalize();
409 
410  // transform points into temp x-y coordinates
411  data_type r, x0, y0, xa, ya, xb, yb, xc, yc;
412  point_type tmp;
413 
414  xa=0;
415  ya=0;
416  tmp=middle-start;
417  xb=tmp.dot(xtmp);
418  yb=tmp.dot(ytmp);
419  tmp=end-start;
420  xc=tmp.dot(xtmp);
421  yc=tmp.dot(ytmp);
422 
423  // use formula to calculate radius and origin (will be in temp x-y coordinates)
424  eli::geom::curve::utility::calculate_circle(r, x0, y0, xa, ya, xb, yb, xc, yc);
425 
426  // calculate the actual coordinates of the origin and set values
427  orig=x0*xtmp+y0*ytmp+start;
428  set(start, orig, normal);
429  }
430  }
431  };
432 
433 
434  template<typename data__, unsigned short dim__, typename tol__>
435  class piecewise_ellipse_creator : public piecewise_ellipse_creator_base<data__, dim__, tol__>
436  {
437  public:
438  typedef data__ data_type;
439  typedef int index_type;
440  typedef Eigen::Matrix<data_type, 1, dim__> point_type;
441  typedef tol__ tolerance_type;
442 
443  public:
444  piecewise_ellipse_creator() : piecewise_ellipse_creator_base<data_type, dim__, tolerance_type>() {}
445  piecewise_ellipse_creator(const index_type &ns) : piecewise_ellipse_creator_base<data_type, dim__, tolerance_type>(ns, 1, 1) {}
447  : piecewise_ellipse_creator_base<data_type, dim__, tolerance_type>(pcc) {}
448 
449  void set_x_axis_radius(const data_type &xr)
450  {
451  tolerance_type tol;
452 
453  if (tol.approximately_equal(xr, 0))
454  {
455  this->set_x_radius(0);
456  }
457  else if (xr<0)
458  {
459  assert(false);
460  }
461  else
462  {
463  this->set_x_radius(xr);
464  }
465  }
466  data_type get_x_axis_radius() const
467  {
468  return this->get_x_radius();
469  }
470 
471  void set_y_axis_radius(const data_type &yr)
472  {
473  tolerance_type tol;
474 
475  if (tol.approximately_equal(yr, 0))
476  {
477  this->set_y_radius(0);
478  }
479  else if (yr<0)
480  {
481  assert(false);
482  }
483  else
484  {
485  this->set_y_radius(yr);
486  }
487  }
488  data_type get_y_axis_radius() const
489  {
490  return this->get_y_radius();
491  }
492 
493  void set(const point_type &orig, const point_type &x, const point_type &y, const data_type &xr, const data_type &yr)
494  {
495  this->set_origin(orig);
496  this->set_xy_directions(x, y);
497  set_x_axis_radius(xr);
498  set_y_axis_radius(yr);
499  }
500  };
501  }
502  }
503 }
504 #endif
void set_x_axis_radius(const data_type &xr)
Definition: piecewise_circle_creator.hpp:449
tol__ tolerance_type
Definition: piecewise_circle_creator.hpp:250
data__ data_type
Definition: piecewise_creator_base.hpp:33
Definition: math.hpp:20
Derived1__::Scalar distance(const Eigen::MatrixBase< Derived1__ > &p1, const Eigen::MatrixBase< Derived2__ > &p2)
Definition: distance.hpp:33
point_type y
Definition: piecewise_circle_creator.hpp:239
piecewise_ellipse_creator()
Definition: piecewise_circle_creator.hpp:444
piecewise_ellipse_creator(const piecewise_ellipse_creator< data_type, dim__, tolerance_type > &pcc)
Definition: piecewise_circle_creator.hpp:446
piecewise_creator_base< data__, dim__, tol__ > base_class_type
Definition: piecewise_circle_creator.hpp:63
void set_origin(const point_type &orig)
Definition: piecewise_circle_creator.hpp:85
data__ data_type
Definition: piecewise_circle_creator.hpp:247
data_type yradius
Definition: piecewise_circle_creator.hpp:240
base_class_type::data_type data_type
Definition: piecewise_circle_creator.hpp:64
virtual ~piecewise_ellipse_creator_base()
Definition: piecewise_circle_creator.hpp:83
base_class_type::tolerance_type tolerance_type
Definition: piecewise_circle_creator.hpp:67
const data_type & get_x_radius() const
Definition: piecewise_circle_creator.hpp:224
piecewise_ellipse_creator_base()
Definition: piecewise_circle_creator.hpp:70
error_code push_back(const curve_type &curve, const data_type &dt=1.0)
Definition: piecewise.hpp:688
void set_x_radius(const data_type &xr)
Definition: piecewise_circle_creator.hpp:213
Definition: piecewise_circle_creator.hpp:435
piecewise_circle_creator(const piecewise_circle_creator< data_type, dim__, tolerance_type > &pcc)
Definition: piecewise_circle_creator.hpp:255
const data_type & get_y_radius() const
Definition: piecewise_circle_creator.hpp:236
data__ data_type
Definition: piecewise_circle_creator.hpp:438
base_class_type::point_type point_type
Definition: piecewise_circle_creator.hpp:65
int index_type
Definition: piecewise_circle_creator.hpp:248
point_type get_origin() const
Definition: piecewise_circle_creator.hpp:86
Definition: math.hpp:25
Definition: piecewise.hpp:244
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: piecewise_creator_base.hpp:34
int index_type
Definition: piecewise_circle_creator.hpp:439
tol__ tolerance_type
Definition: piecewise_circle_creator.hpp:441
Definition: piecewise_circle_creator.hpp:244
index_type get_number_segments() const
Definition: piecewise_creator_base.hpp:47
void set(const point_type &start, const point_type &orig, const point_type &normal)
Definition: piecewise_circle_creator.hpp:328
void set_3pt(const point_type &start, const point_type &middle, const point_type &end)
Definition: piecewise_circle_creator.hpp:367
piecewise_circle_creator()
Definition: piecewise_circle_creator.hpp:253
void set_y_radius(const data_type &yr)
Definition: piecewise_circle_creator.hpp:225
data_type get_y_axis_radius() const
Definition: piecewise_circle_creator.hpp:488
Definition: piecewise_circle_creator.hpp:60
piecewise_ellipse_creator(const index_type &ns)
Definition: piecewise_circle_creator.hpp:445
piecewise_circle_creator(const index_type &ns)
Definition: piecewise_circle_creator.hpp:254
data_type get_segment_dt(const index_type &i) const
Definition: piecewise_creator_base.hpp:78
void clear()
Definition: piecewise.hpp:599
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: piecewise_circle_creator.hpp:249
virtual bool create(piecewise< bezier, data_type, dim__, tolerance_type > &pc) const
Definition: piecewise_circle_creator.hpp:110
data_type get_x_axis_radius() const
Definition: piecewise_circle_creator.hpp:466
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: piecewise_circle_creator.hpp:440
void set_y_axis_radius(const data_type &yr)
Definition: piecewise_circle_creator.hpp:471
tol__ tolerance_type
Definition: piecewise_creator_base.hpp:36
void get_xy_directions(point_type &xdir, point_type &ydir) const
Definition: piecewise_circle_creator.hpp:104
piecewise_ellipse_creator_base(const index_type &ns, const data_type &xr, const data_type &yr)
Definition: piecewise_circle_creator.hpp:75
piecewise_ellipse_creator_base(const piecewise_ellipse_creator_base< data_type, dim__, tolerance_type > &pcc)
Definition: piecewise_circle_creator.hpp:81
void calculate_circle(data__ &rad, data__ &x0, data__ &y0, const data__ &xa, const data__ &ya, const data__ &xb, const data__ &yb, const data__ &xc, const data__ &yc)
Definition: piecewise_circle_creator.hpp:37
point_type x
Definition: piecewise_circle_creator.hpp:239
base_class_type::index_type index_type
Definition: piecewise_circle_creator.hpp:66
void set_t0(const data_type &t0_in)
Definition: piecewise.hpp:340
data_type get_t0() const
Definition: piecewise_creator_base.hpp:62
void set(const point_type &orig, const point_type &x, const point_type &y, const data_type &xr, const data_type &yr)
Definition: piecewise_circle_creator.hpp:493
point_type::Index index_type
Definition: piecewise_creator_base.hpp:35
point_type origin
Definition: piecewise_circle_creator.hpp:239
data_type xradius
Definition: piecewise_circle_creator.hpp:240
void set(const point_type &orig, const point_type &x, const point_type &y, const data_type &r)
Definition: piecewise_circle_creator.hpp:283
void set_xy_directions(const point_type &xdir, const point_type &ydir)
Definition: piecewise_circle_creator.hpp:88
Definition: piecewise_creator_base.hpp:30
void set_radius(const data_type &r)
Definition: piecewise_circle_creator.hpp:258
data_type get_radius() const
Definition: piecewise_circle_creator.hpp:277
void set(const point_type &start, const point_type &orig)
Definition: piecewise_circle_creator.hpp:290