Code-Eli  0.3.6
minimum_distance_curve.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_intersect_minimum_distance_curve_hpp
14 #define eli_geom_intersect_minimum_distance_curve_hpp
15 
16 #include <cmath>
17 #include <vector>
18 #include <list>
19 #include <algorithm>
20 
21 #include "eli/code_eli.hpp"
22 
24 
28 
29 namespace eli
30 {
31  namespace geom
32  {
33  namespace curve
34  {
35  template<template<typename, unsigned short, typename> class curve__, typename data__, unsigned short dim__, typename tol__ >
36  class piecewise;
37  }
38 
39  namespace intersect
40  {
41  namespace internal
42  {
43  template <typename curve__>
45  {
46  const curve__ *pc;
47  typename curve__::point_type pt;
48 
49  typename curve__::data_type operator()(const typename curve__::data_type &t) const
50  {
51  typename curve__::data_type tt(t);
52 
53  if ( !(tt>=pc->get_t0()) )
54  {
55  std::cout << "Minimum distance curve g_functor, tt less than minimum. tt: " << tt << " t0: " << pc->get_t0() << std::endl;
56  tt=pc->get_t0();
57  }
58  if ( !(tt<=pc->get_tmax()) )
59  {
60  std::cout << "Minimum distance curve g_functor, tt greater than maximum. tt: " << tt << " tmax: " << pc->get_tmax() << std::endl;
61  tt=pc->get_tmax();
62  }
63 
64  assert((tt>=pc->get_t0()) && (tt<=pc->get_tmax()));
65 
66  return (pc->f(tt)-pt).dot(pc->fp(tt));
67  }
68  };
69 
70  template <typename curve__>
72  {
73  const curve__ *pc;
74  typename curve__::point_type pt;
75 
76  typename curve__::data_type operator()(const typename curve__::data_type &t) const
77  {
78  typename curve__::data_type tt(t);
79 
80  if ( !(tt>=pc->get_t0()) )
81  {
82  std::cout << "Minimum distance curve gp_functor, tt less than minimum. tt: " << tt << " t0: " << pc->get_t0() << std::endl;
83  tt=pc->get_t0();
84  }
85  if ( !(tt<=pc->get_tmax()) )
86  {
87  std::cout << "Minimum distance curve gp_functor, tt greater than maximum. tt: " << tt << " tmax: " << pc->get_tmax() << std::endl;
88  tt=pc->get_tmax();
89  }
90 
91  assert((tt>=pc->get_t0()) && (tt<=pc->get_tmax()));
92 
93  typename curve__::point_type fp(pc->fp(tt));
94  typename curve__::data_type rtn(fp.dot(fp)+pc->fpp(tt).dot(pc->f(tt)-pt));
95  typename curve__::tolerance_type tol;
96 
97  if (tol.approximately_equal(rtn, 0))
98  {
100 
101  g.pc=pc;
102  g.pt=pt;
103  if (t>=pc->get_tmax())
104  {
105  rtn=(g(pc->get_tmax())-g(static_cast<typename curve__::data_type>(pc->get_tmax()-.01)))/static_cast<typename curve__::data_type>(0.01);
106  }
107  else if (t<=pc->get_t0())
108  {
109  rtn=(g(pc->get_t0()+static_cast<typename curve__::data_type>(0.01))-g(pc->get_t0()))/static_cast<typename curve__::data_type>(0.01);
110  }
111  else
112  {
113  rtn=(g(t+static_cast<typename curve__::data_type>(0.01))-g(t))/static_cast<typename curve__::data_type>(0.01);
114  }
115  }
116 
117  return rtn;
118  }
119  };
120  }
121 
122  template<typename curve__>
123  typename curve__::data_type minimum_distance(typename curve__::data_type &t, const curve__ &c, const typename curve__::point_type &pt, const typename curve__::data_type &t0)
124  {
128  typename curve__::data_type dist0, dist;
129  typename curve__::tolerance_type tol;
130 
131  // setup the functors
132  g.pc=&c;
133  g.pt=pt;
134  gp.pc=&c;
135  gp.pt=pt;
136 
137  // setup the solver
138  nrm.set_absolute_tolerance(tol.get_absolute_tolerance());
139  nrm.set_max_iteration(10);
140  if (c.open())
141  {
144  }
145  else
146  {
147  nrm.set_periodic_condition(c.get_t0(), c.get_tmax());
148  }
149 
150  // set the initial guess
151  nrm.set_initial_guess(t0);
152  dist0=eli::geom::point::distance(c.f(t0), pt);
153 
154  // find the root
155  nrm.find_root(t, g, gp, 0);
156 
157  // if root is within bounds and is closer than initial guess
158  {
159  assert((t>=c.get_t0()) && (t<=c.get_tmax()));
160 
161  dist = eli::geom::point::distance(c.f(t), pt);
162  if (dist<=dist0)
163  {
164  return dist;
165  }
166  }
167 
168  // couldn't find better answer so return initial guess
169  t=t0;
170  return dist0;
171  }
172 
173  template<typename curve__>
174  typename curve__::data_type minimum_distance(typename curve__::data_type &t, const curve__ &c, const typename curve__::point_type &pt)
175  {
176  typename curve__::tolerance_type tol;
177  std::list<std::pair<typename curve__::data_type, typename curve__::data_type>> tinit;
178  typename std::list<std::pair<typename curve__::data_type, typename curve__::data_type>>::iterator it;
179  std::pair<typename curve__::data_type, typename curve__::data_type> cand_pair;
180 
181  // possible that end points are closest, so start by checking them
182  typename curve__::data_type dist, tt, dd, tspan;
183 
184 
185  typename curve__::index_type i, n;
186 
187  // Just a guess
188  n=2*c.degree()+1;
189  tspan = c.get_tmax()-c.get_t0();
190 
191  // Evenly spaced in parameter, don't repeat 0/1 if closed curve.
192  typename curve__::data_type dt;
193  if (c.open())
194  {
195  dt = tspan/(n-1);
196  }
197  else
198  {
199  dt = tspan/n;
200  }
201 
202  // Find closest of evenly spaced points.
203  tt = c.get_t0();
204  dist = std::numeric_limits<typename curve__::data_type>::max();
205  for (i = 0; i < n; i++)
206  {
207  dd=eli::geom::point::distance(c.f(tt), pt);
208 
209  if( dd < dist )
210  {
211  t=tt;
212  dist=dd;
213  }
214  tt+=dt;
215  if( tt >= c.get_tmax() )
216  {
217  tt=c.get_tmax();
218  }
219  }
220 
221  // Polish best point with Newton's method search.
222  typename curve__::data_type t0(t);
223  dist=minimum_distance(t, c, pt, t0);
224 
225  return dist;
226  }
227 
228  template< typename first__, typename second__>
229  bool pairfirstcompare( const std::pair < first__, second__ > &a, const std::pair < first__, second__ > &b )
230  {
231  return ( a.first < b.first );
232  }
233 
234  template<template<typename, unsigned short, typename> class curve__, typename data__, unsigned short dim__, typename tol__>
238  {
240  typedef typename piecewise_type::curve_type curve_type;
241  typedef typename piecewise_type::data_type data_type;
242  typedef typename piecewise_type::bounding_box_type bounding_box_type;
243 
244  typedef typename piecewise_type::segment_collection_type::const_iterator segit;
245 
246  typedef std::vector< std::pair<data_type,segit> > dvec;
247  dvec minbbdist;
248 
249  // Find closest corner of bounding boxes, add them to vector
250  // Simple linear search, would be more efficient with some sort of tree.
251  for (segit seg=pc.segments.begin(); seg!=pc.segments.end(); ++seg)
252  {
253  bounding_box_type bb_local;
254  seg->second.get_bounding_box(bb_local);
255 
256  data_type dbbmin;
257  dbbmin = minimum_distance(bb_local, pt);
258 
259  minbbdist.push_back(std::make_pair(dbbmin, seg));
260  }
261 
262  // Sort by nearest distance.
263  std::sort( minbbdist.begin(), minbbdist.end(), pairfirstcompare<data_type, segit> );
264 
265  // Iterate over segments, starting with nearest bounding box
266  data_type dmin(std::numeric_limits<data_type>::max());
267  typename dvec::const_iterator it;
268  for (it=minbbdist.begin(); it!=minbbdist.end(); ++it)
269  {
270  // If nearest bb distance is farther than current best, we're done.
271  if(it->first < dmin )
272  {
273  segit seg = it->second;
274 
275  curve_type c(seg->second);
276 
277  data_type tlocal, d;
278  d=minimum_distance(tlocal,c,pt);
279 
280  if(d < dmin)
281  {
282  data_type tstart(seg->first);
283  data_type dt(pc.get_delta_t(seg));
284 
285  dmin = d;
286  t=tstart+tlocal*dt;
287  }
288  }
289  else
290  {
291  break;
292  }
293 
294  }
295  return dmin;
296  }
297  }
298  }
299 }
300 #endif
void set_periodic_condition(const data_type &dmin, const data_type &dmax)
Definition: newton_raphson_constrained_method.hpp:58
Definition: math.hpp:20
curve::piecewise< curve1__, data1__, dim1__, tol1__ >::data_type minimum_distance(typename curve::piecewise< curve1__, data1__, dim1__, tol1__ >::data_type &t, const curve::piecewise< curve1__, data1__, dim1__, tol1__ > &pc, const typename curve::piecewise< curve1__, data1__, dim1__, tol1__ >::point_type &pt)
Derived1__::Scalar distance(const Eigen::MatrixBase< Derived1__ > &p1, const Eigen::MatrixBase< Derived2__ > &p2)
Definition: distance.hpp:33
curve__::data_type operator()(const typename curve__::data_type &t) const
Definition: minimum_distance_curve.hpp:49
curve__::point_type pt
Definition: minimum_distance_curve.hpp:74
data__ data_type
Definition: piecewise.hpp:276
curve__::data_type operator()(const typename curve__::data_type &t) const
Definition: minimum_distance_curve.hpp:76
void set_upper_condition(const data_type &d, end_condition_usage ec)
Definition: newton_raphson_constrained_method.hpp:93
Definition: piecewise.hpp:244
void get_bounding_box(bounding_box_type &bb) const
Definition: piecewise.hpp:456
Definition: newton_raphson_constrained_method.hpp:30
void set_max_iteration(const iteration_type &mi)
Definition: iterative_root_base.hpp:207
const curve__ * pc
Definition: minimum_distance_curve.hpp:73
segment_collection_type segments
Definition: piecewise.hpp:1858
void set_absolute_tolerance(const tolerance_type &abs_tol)
Definition: iterative_root_base.hpp:194
Definition: minimum_distance_curve.hpp:71
curve__::point_type pt
Definition: minimum_distance_curve.hpp:47
int find_root(data_type &root, const f__ &fun, const g__ &fprime, const data_type &f0) const
Definition: newton_raphson_method.hpp:59
void set_lower_condition(const data_type &d, end_condition_usage ec)
Definition: newton_raphson_constrained_method.hpp:72
const curve__ * pc
Definition: minimum_distance_curve.hpp:46
bool pairfirstcompare(const std::pair< first__, second__ > &a, const std::pair< first__, second__ > &b)
Definition: minimum_distance_curve.hpp:229
curve_type::point_type point_type
Definition: piecewise.hpp:272
Definition: minimum_distance_curve.hpp:44
void set_initial_guess(const data_type &xg)
Definition: newton_raphson_method.hpp:48
data_type get_delta_t(const typename segment_collection_type::iterator &it) const
Definition: piecewise.hpp:1939