Code-Eli  0.3.6
fit_container.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 geom_curve_fit_container_hpp
14 #define geom_curve_fit_container_hpp
15 
16 #include <map>
17 
18 #include "eli/code_eli.hpp"
19 
20 #include "eli/mutil/fd/d1o2.hpp"
21 #include "eli/mutil/fd/d2o2.hpp"
22 
25 
26 namespace eli
27 {
28  namespace geom
29  {
30  namespace curve
31  {
32 
33  template<typename data_type__, typename index_type__, size_t dim__, size_t con_dim__>
35  {
36  public:
37  typedef data_type__ data_type;
38  typedef index_type__ index_type;
39  typedef Eigen::Matrix<data_type, 1, dim__> point_type;
40 
42  {
48  };
49 
51  {
52  public:
53  typedef Eigen::Matrix<data_type, 1, con_dim__> point_type;
54 
56  {
57  NOT_USED =-1,
58  SET = 0,
59  FD = 1
60  };
61 
62  private:
63  point_type fp, fpp;
65 
66  public:
67  constraint_info() : use_fp(NOT_USED), use_fpp(NOT_USED) {}
68  constraint_info(const constraint_info &ci) : fp(ci.fp), fpp(ci.fpp), use_fp(ci.use_fp), use_fpp(ci.use_fpp) {}
70  public:
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 
74  {
75  if (this!=&ci)
76  {
77  fp=ci.fp;
78  fpp=ci.fpp;
79  use_fp=ci.use_fp;
80  use_fpp=ci.use_fpp;
81  }
82 
83  return *this;
84  }
85 
86  point_type get_fp() const {return fp;}
87  point_type get_fpp() const {return fpp;}
88  use_states using_fp() const {return use_fp;}
89  use_states using_fpp() const {return use_fpp;}
90 
91  void set_fp(const point_type &p, bool fd)
92  {
93  if (fd)
94  use_fp=FD;
95  else
96  use_fp=SET;
97 
98  fp=p;
99  }
100 
101  void set_fpp(const point_type &p, bool pfd, const point_type &pp, bool ppfd)
102  {
103  if (pfd)
104  use_fp=FD;
105  else
106  use_fp=SET;
107  fp=p;
108 
109  if (ppfd)
110  use_fpp=FD;
111  else
112  use_fpp=SET;
113  fpp=pp;
114  }
115  };
116 
118 
119  private:
121  std::vector<point_type, Eigen::aligned_allocator<point_type> > points;
122  typedef std::map<index_type, constraint_info> constraint_collection;
123 
124  constraint_collection constraints;
125 
126  private:
127  bool valid_index(const index_type &i) const
128  {
129  if ( (i>=0) && (i<static_cast<index_type>(number_points())) )
130  return true;
131  else
132  return false;
133  }
134 
135  void evaluate_fp(constraint_point_type &fp, const index_type &i) const
136  {
137  // need to do finite difference to calculate 1st derivative vector
138  typedef eli::mutil::fd::d1o2<data_type> fd1_type;
139  fd1_type d;
140  point_type vec[3];
141  data_type t[3];
142 
143  if (i==0)
144  {
145  if (open())
146  {
147  d.set_stencil(fd1_type::RIGHT);
148 
149  // set points
150  vec[0]=points[0];
151  vec[1]=points[1];
152  vec[2]=points[2];
153  }
154  else
155  {
156  d.set_stencil(fd1_type::CENTER);
157 
158  // set points
159  vec[0]=points[number_points()-1];
160  vec[1]=points[0];
161  vec[2]=points[1];
162  }
163  }
164  else if (i==static_cast<index_type>(number_points())-1)
165  {
166  if (open())
167  {
168  d.set_stencil(fd1_type::LEFT);
169 
170  // set points
171  vec[0]=points[number_points()-3];
172  vec[1]=points[number_points()-2];
173  vec[2]=points[number_points()-1];
174  }
175  else
176  {
177  d.set_stencil(fd1_type::CENTER);
178 
179  // set points
180  vec[0]=points[number_points()-2];
181  vec[1]=points[number_points()-1];
182  vec[2]=points[0];
183  }
184  }
185  else
186  {
187  d.set_stencil(fd1_type::CENTER);
188 
189  // set points
190  vec[0]=points[i-1];
191  vec[1]=points[i];
192  vec[2]=points[i+1];
193  }
194 
195  // calculate the distances between points
196  size_t joff;
197  if (dim__==con_dim__)
198  {
199  joff=0;
200  t[0]=0;
201  t[1]=eli::geom::point::distance(vec[1], vec[0]);
202  t[2]=eli::geom::point::distance(vec[2], vec[1]);
203  t[2]+=t[1];
204  }
205  else
206  {
207  joff=1;
208  t[0]=vec[0](0);
209  t[1]=vec[1](0);
210  t[2]=vec[2](0);
211  }
212 
213  // calculate the approximate derivative
214  data_type y[3], fpj(0);
215  for (size_t j=0; j<con_dim__; ++j)
216  {
217  y[0]=vec[0](j+joff);
218  y[1]=vec[1](j+joff);
219  y[2]=vec[2](j+joff);
220  d.evaluate(fpj, y, t);
221  fp(j)=fpj;
222  }
223  }
224 
225  void evaluate_fpp(constraint_point_type &fpp, const index_type &i) const
226  {
227  // need to do finite difference to calculate 1st derivative vector
228  typedef eli::mutil::fd::d2o2<data_type> fd2_type;
229  fd2_type d;
230  point_type vec[4];
231  data_type t[4];
232 
233  if (i==0)
234  {
235  if (open())
236  {
237  d.set_stencil(fd2_type::RIGHT);
238 
239  // set points
240  vec[0]=points[0];
241  vec[1]=points[1];
242  vec[2]=points[2];
243  vec[3]=points[3];
244  }
245  else
246  {
247  d.set_stencil(fd2_type::LEFT_BIASED);
248 
249  // set points
250  vec[0]=points[number_points()-2];
251  vec[1]=points[number_points()-1];
252  vec[2]=points[0];
253  vec[3]=points[1];
254  }
255  }
256  else if (i==1)
257  {
258  if (open())
259  {
260  d.set_stencil(fd2_type::RIGHT_BIASED);
261 
262  // set points
263  vec[0]=points[0];
264  vec[1]=points[1];
265  vec[2]=points[2];
266  vec[3]=points[3];
267  }
268  else
269  {
270  d.set_stencil(fd2_type::LEFT_BIASED);
271 
272  // set points
273  vec[0]=points[number_points()-1];
274  vec[1]=points[0];
275  vec[2]=points[1];
276  vec[3]=points[2];
277  }
278  }
279  else if (i==static_cast<index_type>(number_points())-2)
280  {
281  if (open())
282  {
283  d.set_stencil(fd2_type::LEFT_BIASED);
284 
285  // set points
286  vec[0]=points[number_points()-4];
287  vec[1]=points[number_points()-3];
288  vec[2]=points[number_points()-2];
289  vec[3]=points[number_points()-1];
290  }
291  else
292  {
293  d.set_stencil(fd2_type::RIGHT_BIASED);
294 
295  // set points
296  vec[0]=points[number_points()-3];
297  vec[1]=points[number_points()-2];
298  vec[2]=points[number_points()-1];
299  vec[3]=points[0];
300  }
301  }
302  else if (i==static_cast<index_type>(number_points())-1)
303  {
304  if (open())
305  {
306  d.set_stencil(fd2_type::LEFT);
307 
308  // set points
309  vec[0]=points[number_points()-4];
310  vec[1]=points[number_points()-3];
311  vec[2]=points[number_points()-2];
312  vec[3]=points[number_points()-1];
313  }
314  else
315  {
316  d.set_stencil(fd2_type::RIGHT_BIASED);
317 
318  // set points
319  vec[0]=points[number_points()-2];
320  vec[1]=points[number_points()-1];
321  vec[2]=points[0];
322  vec[3]=points[1];
323  }
324  }
325  else
326  {
327  d.set_stencil(fd2_type::RIGHT_BIASED);
328 
329  // set points
330  vec[0]=points[i-1];
331  vec[1]=points[i];
332  vec[2]=points[i+1];
333  vec[3]=points[i+2];
334  }
335 
336  // calculate the distances between points
337  size_t joff;
338  if (dim__==con_dim__)
339  {
340  joff=0;
341  t[0]=0;
342  t[1]=eli::geom::point::distance(vec[1], vec[0]);
343  t[2]=eli::geom::point::distance(vec[2], vec[1]);
344  t[2]+=t[1];
345  t[3]=eli::geom::point::distance(vec[3], vec[2]);
346  t[3]+=t[2];
347  }
348  else
349  {
350  joff=1;
351  t[0]=vec[0](0);
352  t[1]=vec[1](0);
353  t[2]=vec[2](0);
354  t[3]=vec[3](0);
355  }
356 
357  // calculate the approximate derivative
358  data_type y[4], fppj(0);
359  for (size_t j=0; j<con_dim__; ++j)
360  {
361  y[0]=vec[0](j+joff);
362  y[1]=vec[1](j+joff);
363  y[2]=vec[2](j+joff);
364  y[3]=vec[3](j+joff);
365  d.evaluate(fppj, y, t);
366  fpp(j)=fppj;
367  }
368  }
369 
370  public:
371  fit_container() : end_continuity(eli::geom::general::NOT_CONNECTED) {}
373 
374  size_t number_points() const {return points.size();}
375  size_t number_constraint_points() const {return constraints.size();}
376 
377  size_t number_constraints(bool fit=true) const
378  {
379  typename constraint_collection::const_iterator it;
380  size_t ncon(0), nfit=fit?1:0;
381 
382  for (it=constraints.begin(); it!=constraints.end(); ++it)
383  {
384  if (it->second.using_fp()==constraint_info::NOT_USED)
385  ncon+=nfit;
386  else if (it->second.using_fpp()==constraint_info::NOT_USED)
387  ncon+=nfit+1;
388  else
389  ncon+=nfit+2;
390  }
391 
392  return ncon;
393  }
394 
395  bool open() const {return end_continuity==eli::geom::general::NOT_CONNECTED;}
396  bool closed() const {return end_continuity!=eli::geom::general::NOT_CONNECTED;}
398 
400  {
401  // if changing flag, then need to check start and end constraints to see if the
402  // finite differences need to be redone
403  if (op!=get_end_flag())
404  {
405  index_type indx[4];
406  typename constraint_collection::const_iterator it;
407 
408  // set the open flag
409  end_continuity=op;
410 
411  // these are the four indexes that need to be checked for
412  indx[0]=0;
413  indx[1]=1;
414  indx[2]=static_cast<index_type>(number_points())-2;
415  indx[3]=static_cast<index_type>(number_points())-1;
416 
417  // check the start and end constraints need to be recalculated
418  for (size_t i=0; i<4; ++i)
419  {
420  it=constraints.find(indx[i]);
421  if (it!=constraints.end())
422  {
423  // if first derivative is finite difference, then either both are, or the
424  // second derivative is not set
425  if (it->second.using_fp()==constraint_info::FD)
426  {
427  // check if second derivative is set
428  if (it->second.using_fpp()==constraint_info::FD)
429  add_C2_constraint(indx[i]);
430  else
431  add_C1_constraint(indx[i]);
432  }
433  // if second derivative is finite difference, then must have set first derivative
434  else if (it->second.using_fpp()==constraint_info::FD)
435  {
436  add_C2_constraint(indx[i], it->second.get_fp());
437  }
438  }
439  }
440  }
441  }
442 
443  template<typename it__>
444  void set_points(it__ itb, it__ ite)
445  {
446  points.clear();
447  points.insert(points.begin(), itb, ite);
448  }
449 
450  template<typename it__>
451  void get_points(it__ itb) const
452  {
453  std::copy(points.begin(), points.end(), itb);
454  }
455 
457  {
458  return get_constraint(0, ci);
459  }
460 
462  {
463  return get_constraint(0, ci, pt);
464  }
465 
467  {
468  return get_constraint(number_points()-1, ci);
469  }
470 
471  error_code get_end_constraint(constraint_info &ci, point_type &pt) const
472  {
473  return get_constraint(number_points()-1, ci, pt);
474  }
475 
476  error_code get_constraint(const index_type &i, constraint_info &ci) const
477  {
478  // check if have valid index
479  if (!valid_index(i))
480  return INVALID_INDEX;
481 
482  typename constraint_collection::const_iterator it=constraints.find(i);
483  if (it==constraints.end())
484  return INDEX_NOT_FOUND;
485 
486  ci=it->second;
487 
488  return NO_ERRORS;
489  }
490 
491  error_code get_constraint(const index_type &i, constraint_info &ci, point_type &pt) const
492  {
493  error_code ec;
494 
495  ec=get_constraint(i, ci);
496  if (ec==NO_ERRORS)
497  pt=points[i];
498 
499  return NO_ERRORS;
500  }
501 
503  {
504  return add_C0_constraint(0);
505  }
506 
508  {
509  return add_C1_constraint(0);
510  }
511 
512  error_code add_start_C1_constraint(const constraint_point_type &fp)
513  {
514  return add_C1_constraint(0, fp);
515  }
516 
518  {
519  return add_C2_constraint(0);
520  }
521 
522  error_code add_start_C2_constraint(const constraint_point_type &fp)
523  {
524  return add_C2_constraint(0, fp);
525  }
526 
527  error_code add_start_C2_constraint(const constraint_point_type &fp, const constraint_point_type &fpp)
528  {
529  return add_C2_constraint(0, fp, fpp);
530  }
531 
533  {
534  return add_C0_constraint(static_cast<int>(number_points())-1);
535  }
536 
538  {
539  return add_C1_constraint(static_cast<int>(number_points())-1);
540  }
541 
542  error_code add_end_C1_constraint(const constraint_point_type &fp)
543  {
544  return add_C1_constraint(static_cast<int>(number_points())-1, fp);
545  }
546 
548  {
549  return add_C2_constraint(static_cast<int>(number_points())-1);
550  }
551 
552  error_code add_end_C2_constraint(const constraint_point_type &fp)
553  {
554  return add_C2_constraint(static_cast<int>(number_points())-1, fp);
555  }
556 
557  error_code add_end_C2_constraint(const constraint_point_type &fp, const constraint_point_type &fpp)
558  {
559  return add_C2_constraint(static_cast<int>(number_points())-1, fp, fpp);
560  }
561 
562  error_code add_C0_constraint(const index_type &i)
563  {
564  // check if have valid index
565  if (!valid_index(i))
566  return INVALID_INDEX;
567 
568  // this will either create or replace
569  constraint_info ci;
570  constraints[i]=ci;
571 
572  return NO_ERRORS;
573  }
574 
575  error_code add_C1_constraint(const index_type &i)
576  {
577  // check if have valid index
578  if (!valid_index(i))
579  return INVALID_INDEX;
580 
581  // check if have enough points (3 for finite difference of 1st derivative)
582  if (number_points()<3)
583  return TOO_FEW_POINTS;
584 
585  // need to do finite difference to calculate 1st derivative vector
586  constraint_point_type fp;
587  evaluate_fp(fp, i);
588 
589  // this will either create or replace
590  constraint_info ci;
591  ci.set_fp(fp, true);
592  constraints[i]=ci;
593 
594  return NO_ERRORS;
595  }
596 
597  error_code add_C1_constraint(const index_type &i, const constraint_point_type &fp)
598  {
599  // check if have valid index
600  if (!valid_index(i))
601  return INVALID_INDEX;
602 
603  // this will either create or replace
604  constraint_info ci;
605  ci.set_fp(fp, false);
606  constraints[i]=ci;
607 
608  return NO_ERRORS;
609  }
610 
611  error_code add_C2_constraint(const index_type &i)
612  {
613  // check if have valid index
614  if (!valid_index(i))
615  return INVALID_INDEX;
616 
617  // check if have enough points (4 for finite difference of 2nd derivative)
618  if (number_points()<4)
619  return TOO_FEW_POINTS;
620 
621  // need to do finite difference to calculate 1st derivative vector
622  constraint_point_type fp;
623  evaluate_fp(fp, i);
624 
625  // need to do finite difference to calculate 2nd derivative vector
626  constraint_point_type fpp;
627  evaluate_fpp(fpp, i);
628 
629  // this will either create or replace
630  constraint_info ci;
631  ci.set_fpp(fp, true, fpp, true);
632  constraints[i]=ci;
633 
634  return NO_ERRORS;
635  }
636 
637  error_code add_C2_constraint(const index_type &i, const constraint_point_type &fp)
638  {
639  // check if have valid index
640  if (!valid_index(i))
641  return INVALID_INDEX;
642 
643  // check if have enough points (4 for finite difference of 2nd derivative)
644  if (number_points()<4)
645  return TOO_FEW_POINTS;
646 
647  // need to do finite difference to calculate 2nd derivative vector
648  constraint_point_type fpp;
649  evaluate_fpp(fpp, i);
650 
651  // this will either create or replace
652  constraint_info ci;
653  ci.set_fpp(fp, false, fpp, true);
654  constraints[i]=ci;
655 
656  return NO_ERRORS;
657  }
658 
659  error_code add_C2_constraint(const index_type &i, const constraint_point_type &fp, const constraint_point_type &fpp)
660  {
661  // check if have valid index
662  if (!valid_index(i))
663  return INVALID_INDEX;
664 
665  // this will either create or replace
666  constraint_info ci;
667  ci.set_fpp(fp, false, fpp, false);
668  constraints[i]=ci;
669 
670  return NO_ERRORS;
671  }
672 
673  error_code remove_constraint(const index_type &i)
674  {
675  // check if have valid index
676  if (!valid_index(i))
677  return INVALID_INDEX;
678 
679  typename constraint_collection::iterator it=constraints.find(i);
680  if (it==constraints.end())
681  return INDEX_NOT_FOUND;
682 
683  constraints.erase(it);
684  return NO_ERRORS;
685  }
686 
687  template<typename it__>
689  {
690  typename constraint_collection::const_iterator it;
691  for (it=constraints.begin(); it!=constraints.end(); ++it, ++itout)
692  (*itout)=it->first;
693 
694  return NO_ERRORS;
695  }
696  };
697  }
698  }
699 }
700 
701 #endif
constraint_info(const constraint_info &ci)
Definition: fit_container.hpp:68
point_type fpp
Definition: fit_container.hpp:63
error_code get_constraint(const index_type &i, constraint_info &ci, point_type &pt) const
Definition: fit_container.hpp:491
Definition: fit_container.hpp:46
Definition: continuity.hpp:26
Definition: math.hpp:20
use_states use_fpp
Definition: fit_container.hpp:64
Derived1__::Scalar distance(const Eigen::MatrixBase< Derived1__ > &p1, const Eigen::MatrixBase< Derived2__ > &p2)
Definition: distance.hpp:33
error_code add_start_C2_constraint()
Definition: fit_container.hpp:517
use_states using_fp() const
Definition: fit_container.hpp:88
Eigen::Matrix< data_type, 1, con_dim__ > point_type
Definition: fit_container.hpp:53
void set_stencil(const stencil &s)
Definition: d1o2.hpp:70
void set_fpp(const point_type &p, bool pfd, const point_type &pp, bool ppfd)
Definition: fit_container.hpp:101
data_type__ data_type
Definition: fit_container.hpp:37
constraint_info::point_type constraint_point_type
Definition: fit_container.hpp:117
error_code add_start_C1_constraint(const constraint_point_type &fp)
Definition: fit_container.hpp:512
error_code add_C0_constraint(const index_type &i)
Definition: fit_container.hpp:562
void set_stencil(const stencil &s)
Definition: d2o2.hpp:74
Definition: fit_container.hpp:45
size_t number_constraint_points() const
Definition: fit_container.hpp:375
bool valid_index(const index_type &i) const
Definition: fit_container.hpp:127
std::map< index_type, constraint_info > constraint_collection
Definition: fit_container.hpp:122
error_code add_C2_constraint(const index_type &i)
Definition: fit_container.hpp:611
error_code get_end_constraint(constraint_info &ci) const
Definition: fit_container.hpp:466
fit_container()
Definition: fit_container.hpp:371
size_t number_points() const
Definition: fit_container.hpp:374
error_code get_end_constraint(constraint_info &ci, point_type &pt) const
Definition: fit_container.hpp:471
EIGEN_MAKE_ALIGNED_OPERATOR_NEW constraint_info & operator=(const constraint_info &ci)
Definition: fit_container.hpp:73
Definition: fit_container.hpp:34
error_code add_end_C2_constraint(const constraint_point_type &fp, const constraint_point_type &fpp)
Definition: fit_container.hpp:557
error_code add_C1_constraint(const index_type &i, const constraint_point_type &fp)
Definition: fit_container.hpp:597
error_code get_start_constraint(constraint_info &ci) const
Definition: fit_container.hpp:456
use_states using_fpp() const
Definition: fit_container.hpp:89
error_code add_start_C2_constraint(const constraint_point_type &fp, const constraint_point_type &fpp)
Definition: fit_container.hpp:527
void set_fp(const point_type &p, bool fd)
Definition: fit_container.hpp:91
error_code get_start_constraint(constraint_info &ci, point_type &pt) const
Definition: fit_container.hpp:461
error_code add_start_C0_constraint()
Definition: fit_container.hpp:502
Definition: fit_container.hpp:43
error_code remove_constraint(const index_type &i)
Definition: fit_container.hpp:673
error_code add_end_C0_constraint()
Definition: fit_container.hpp:532
error_code add_start_C1_constraint()
Definition: fit_container.hpp:507
use_states
Definition: fit_container.hpp:55
error_code get_constraint(const index_type &i, constraint_info &ci) const
Definition: fit_container.hpp:476
Definition: fit_container.hpp:44
std::vector< point_type, Eigen::aligned_allocator< point_type > > points
Definition: fit_container.hpp:121
point_type get_fpp() const
Definition: fit_container.hpp:87
const eli::geom::general::continuity & get_end_flag() const
Definition: fit_container.hpp:397
constraint_collection constraints
Definition: fit_container.hpp:124
void evaluate_fpp(constraint_point_type &fpp, const index_type &i) const
Definition: fit_container.hpp:225
void evaluate_fp(constraint_point_type &fp, const index_type &i) const
Definition: fit_container.hpp:135
Definition: fit_container.hpp:47
point_type fp
Definition: fit_container.hpp:63
error_code add_C1_constraint(const index_type &i)
Definition: fit_container.hpp:575
void set_end_flag(const eli::geom::general::continuity &op)
Definition: fit_container.hpp:399
~constraint_info()
Definition: fit_container.hpp:69
error_code add_end_C2_constraint(const constraint_point_type &fp)
Definition: fit_container.hpp:552
Definition: d1o2.hpp:27
Definition: d2o2.hpp:27
error_code add_start_C2_constraint(const constraint_point_type &fp)
Definition: fit_container.hpp:522
use_states use_fp
Definition: fit_container.hpp:64
point_type get_fp() const
Definition: fit_container.hpp:86
index_type__ index_type
Definition: fit_container.hpp:38
~fit_container()
Definition: fit_container.hpp:372
error_code add_end_C1_constraint(const constraint_point_type &fp)
Definition: fit_container.hpp:542
continuity
Definition: continuity.hpp:24
error_code
Definition: fit_container.hpp:41
bool open() const
Definition: fit_container.hpp:395
Definition: fit_container.hpp:50
void get_points(it__ itb) const
Definition: fit_container.hpp:451
error_code add_end_C2_constraint()
Definition: fit_container.hpp:547
void set_points(it__ itb, it__ ite)
Definition: fit_container.hpp:444
error_code add_C2_constraint(const index_type &i, const constraint_point_type &fp, const constraint_point_type &fpp)
Definition: fit_container.hpp:659
error_code add_end_C1_constraint()
Definition: fit_container.hpp:537
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
Eigen::Matrix< data_type, 1, dim__ > point_type
Definition: fit_container.hpp:39
bool closed() const
Definition: fit_container.hpp:396
constraint_info()
Definition: fit_container.hpp:67
error_code add_C2_constraint(const index_type &i, const constraint_point_type &fp)
Definition: fit_container.hpp:637
eli::geom::general::continuity end_continuity
Definition: fit_container.hpp:120