Code-Eli  0.3.6
minimum_distance_surface.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_surface_hpp
14 #define eli_geom_intersect_minimum_distance_surface_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 intersect
34  {
35  namespace internal
36  {
37  template <typename surface__>
39  {
40  const surface__ *ps;
41  typename surface__::point_type pt;
42  typedef typename Eigen::Matrix<typename surface__::data_type, 2, 1> vec;
43 
44  vec operator()(const vec &u) const
45  {
46  typename surface__::data_type uu(u[0]), vv(u[1]);
47  vec rtn;
48 
49  typename surface__::data_type umin, umax, vmin, vmax;
50  ps->get_parameter_min(umin,vmin);
51  ps->get_parameter_max(umax,vmax);
52 
53  if ( !(uu>=umin) )
54  {
55  std::cout << "Minimum distance surface g_functor, u less than minimum. uu: " << uu << " umin: " << umin << std::endl;
56  uu=umin;
57  }
58  if ( !(uu<=umax) )
59  {
60  std::cout << "Minimum distance surface g_functor, u greater than maximum. uu: " << uu << " uamx: " << umax << std::endl;
61  uu=umax;
62  }
63 
64  if ( !(vv>=vmin) )
65  {
66  std::cout << "Minimum distance surface g_functor, v less than minimum. vv: " << vv << " vmin: " << vmin << std::endl;
67  vv=vmin;
68  }
69  if ( !(vv<=vmax) )
70  {
71  std::cout << "Minimum distance surface g_functor, v greater than maximum. vv: " << vv << " vmax: " << vmax << std::endl;
72  vv=vmax;
73  }
74 
75  assert((uu>=umin) && (uu<=umax));
76  assert((vv>=vmin) && (vv<=vmax));
77 
78  uu=std::min(std::max(uu, static_cast<typename surface__::data_type>(umin)), static_cast<typename surface__::data_type>(umax));
79  vv=std::min(std::max(vv, static_cast<typename surface__::data_type>(vmin)), static_cast<typename surface__::data_type>(vmax));
80 
81  typename surface__::point_type tmp;
82 
83  tmp=ps->f(uu, vv)-pt;
84  rtn(0)=tmp.dot(ps->f_u(uu, vv));
85  rtn(1)=tmp.dot(ps->f_v(uu, vv));
86  return rtn;
87  }
88  };
89 
90  template <typename surface__>
92  {
93  const surface__ *ps;
94  typename surface__::point_type pt;
95  typedef typename Eigen::Matrix<typename surface__::data_type, 2, 1> vec;
96  typedef typename Eigen::Matrix<typename surface__::data_type, 2, 2> mat;
97 
98  mat operator()(const vec &u) const
99  {
100  typename surface__::data_type uu(u[0]), vv(u[1]);
101  mat rtn;
102 
103  typename surface__::data_type umin, umax, vmin, vmax;
104  ps->get_parameter_min(umin,vmin);
105  ps->get_parameter_max(umax,vmax);
106 
107  if ( !(uu>=umin) )
108  {
109  std::cout << "Minimum distance surface gp_functor, u less than minimum. uu: " << uu << " umin: " << umin << std::endl;
110  uu=umin;
111  }
112  if ( !(uu<=umax) )
113  {
114  std::cout << "Minimum distance surface gp_functor, u greater than maximum. uu: " << uu << " uamx: " << umax << std::endl;
115  uu=umax;
116  }
117 
118  if ( !(vv>=vmin) )
119  {
120  std::cout << "Minimum distance surface gp_functor, v less than minimum. vv: " << vv << " vmin: " << vmin << std::endl;
121  vv=vmin;
122  }
123  if ( !(vv<=vmax) )
124  {
125  std::cout << "Minimum distance surface gp_functor, v greater than maximum. vv: " << vv << " vmax: " << vmax << std::endl;
126  vv=vmax;
127  }
128 
129  assert((uu>=umin) && (uu<=umax));
130  assert((vv>=vmin) && (vv<=vmax));
131 
132  uu=std::min(std::max(uu, static_cast<typename surface__::data_type>(umin)), static_cast<typename surface__::data_type>(umax));
133  vv=std::min(std::max(vv, static_cast<typename surface__::data_type>(vmin)), static_cast<typename surface__::data_type>(vmax));
134 
135  typename surface__::point_type tmp, Su, Sv, Suu, Suv, Svv;
136 
137  tmp=ps->f(uu, vv)-pt;
138  Su=ps->f_u(uu, vv);
139  Sv=ps->f_v(uu, vv);
140  Suu=ps->f_uu(uu, vv);
141  Suv=ps->f_uv(uu, vv);
142  Svv=ps->f_vv(uu, vv);
143 
144  rtn(0,0)=Su.dot(Su)+tmp.dot(Suu);
145  rtn(0,1)=Su.dot(Sv)+tmp.dot(Suv);
146  rtn(1,0)=rtn(0,1);
147  rtn(1,1)=Sv.dot(Sv)+tmp.dot(Svv);
148 
149  // TODO: What to do if matrix becomes singular?
150 
151  return rtn;
152  }
153  };
154  }
155 
156  template<typename surface__>
157  typename surface__::data_type minimum_distance(typename surface__::data_type &u, typename surface__::data_type &v, const surface__ &s, const typename surface__::point_type &pt,
158  const typename surface__::data_type &u0, const typename surface__::data_type &v0)
159  {
161  nonlinear_solver_type nrm;
164  typename surface__::data_type dist0, dist;
165  typename surface__::tolerance_type tol;
166 
167  typename surface__::data_type umin, umax, vmin, vmax;
168  s.get_parameter_min(umin,vmin);
169  s.get_parameter_max(umax,vmax);
170 
171  // setup the functors
172  g.ps=&s;
173  g.pt=pt;
174  gp.ps=&s;
175  gp.pt=pt;
176 
177  // setup the solver
178  nrm.set_absolute_tolerance(tol.get_absolute_tolerance());
179  nrm.set_max_iteration(20);
180  nrm.set_norm_type(nonlinear_solver_type::max_norm);
181  if (s.open_u())
182  {
183  nrm.set_lower_condition(0, umin, nonlinear_solver_type::NRC_EXCLUSIVE);
184  nrm.set_upper_condition(0, umax, nonlinear_solver_type::NRC_EXCLUSIVE);
185  }
186  else
187  {
188  nrm.set_periodic_condition(0, umin, umax);
189  }
190  if (s.open_v())
191  {
192  nrm.set_lower_condition(1, vmin, nonlinear_solver_type::NRC_EXCLUSIVE);
193  nrm.set_upper_condition(1, vmax, nonlinear_solver_type::NRC_EXCLUSIVE);
194  }
195  else
196  {
197  nrm.set_periodic_condition(1, vmin, vmax);
198  }
199 
200  // set the initial guess
201  typename nonlinear_solver_type::solution_matrix uinit, rhs, ans;
202 
203  uinit(0)=u0;
204  uinit(1)=v0;
205  nrm.set_initial_guess(uinit);
206  rhs.setZero();
207  dist0=eli::geom::point::distance(s.f(u0, v0), pt);
208 
209  // find the root
210  nrm.find_root(ans, g, gp, rhs);
211  u=ans(0);
212  v=ans(1);
213 
214  // if root is within bounds and is closer than initial guess
215  {
216  assert((u>=umin) && (u<=umax));
217  assert((v>=vmin) && (v<=vmax));
218 
219  dist = eli::geom::point::distance(s.f(u, v), pt);
220  if (dist<=dist0)
221  {
222  return dist;
223  }
224  }
225 // else
226 // {
227 // std::cout << "% not converged";
228 // if (stat==nonlinear_solver_type::hit_constraint)
229 // std::cout << " because hit constraint" << std::endl;
230 // else if (stat==nonlinear_solver_type::max_iteration)
231 // std::cout << " reached max iteration" << std::endl;
232 // else
233 // std::cout << " for out of range parameters (" << ans(0) << ", " << ans(1) << ")" << std::endl;
234 // }
235 
236  // couldn't find better answer so return initial guess
237  u=u0;
238  v=v0;
239  return dist0;
240  }
241 
242  template<typename surface__>
243  typename surface__::data_type minimum_distance(typename surface__::data_type &u, typename surface__::data_type &v, const surface__ &s, const typename surface__::point_type &pt)
244  {
245  typename surface__::tolerance_type tol;
246 
247  // possible that end points are closest, so start by checking them
248  typename surface__::data_type dist, uu, vv, dd;
249 
250  typename surface__::data_type umin, umax, vmin, vmax, uspan, vspan;
251  s.get_parameter_min(umin,vmin);
252  s.get_parameter_max(umax,vmax);
253  uspan=umax-umin;
254  vspan=vmax-vmin;
255 
256  typename surface__::index_type i, j, nu, nv;
257  typename surface__::data_type du, dv;
258 
259  nu=2*s.degree_u()+1;
260  nv=2*s.degree_v()+1;
261 
262  // Evenly spaced in parameter, don't repeat 0/1 if closed curve.
263  if (s.open_u())
264  {
265  du = uspan/(nu-1);
266  }
267  else
268  {
269  du = uspan/nu;
270  }
271 
272  if (s.open_v())
273  {
274  dv = vspan/(nv-1);
275  }
276  else
277  {
278  dv = vspan/nv;
279  }
280 
281  // Find closest of evenly spaced points.
282  uu=umin;
283  dist = std::numeric_limits<typename surface__::data_type>::max();
284  for (i = 0; i < nu; i++)
285  {
286  vv=vmin;
287  for (j = 0; j < nv; j++)
288  {
289  dd=eli::geom::point::distance(s.f(uu,vv), pt);
290 
291  if( dd < dist )
292  {
293  u=uu;
294  v=vv;
295  dist=dd;
296  }
297  uu+=du;
298  vv+=dv;
299  if(uu>=umax)
300  {
301  uu=umax;
302  }
303  if(vv>=vmax)
304  {
305  vv=vmax;
306  }
307  }
308  }
309 
310  // Polish best point with Newton's method search.
311  dd=minimum_distance(uu, vv, s, pt, u, v);
312 
313  if ((uu>=umin) && (uu<=umax) && (vv>=vmin) && (vv<=vmax))
314  {
315  if (dd<dist)
316  {
317  u=uu;
318  v=vv;
319  dist=dd;
320  }
321  }
322 
323  // next check edges
324  // Since these are always edges, we could implement an edge curve extraction routine
325  // that returned the control points directly instead of performing an arbitrary curve
326  // extraction calculation.
327  typename surface__::curve_type bc;
328  if(u<=(umin+std::abs(umin)*2*std::numeric_limits<typename surface__::data_type>::epsilon()))
329  {
330  s.get_uconst_curve(bc, umin);
331  dd=eli::geom::intersect::minimum_distance(vv, bc, pt, v);
332 
333  if (dd<dist)
334  {
335  u=umin;
336  v=vv;
337  dist=dd;
338  }
339  }
340 
341  if(u>=(umax-std::abs(umax)*2*std::numeric_limits<typename surface__::data_type>::epsilon()))
342  {
343  s.get_uconst_curve(bc, umax);
344  dd=eli::geom::intersect::minimum_distance(vv, bc, pt, v);
345 
346  if (dd<dist)
347  {
348  u=umax;
349  v=vv;
350  dist=dd;
351  }
352  }
353 
354  if(v<=(vmin+std::abs(vmin)*2*std::numeric_limits<typename surface__::data_type>::epsilon()))
355  {
356  s.get_vconst_curve(bc, vmin);
357  dd=eli::geom::intersect::minimum_distance(uu, bc, pt, u);
358 
359  if (dd<dist)
360  {
361  u=uu;
362  v=vmin;
363  dist=dd;
364  }
365  }
366 
367  if(v>=(vmax-std::abs(vmax)*2*std::numeric_limits<typename surface__::data_type>::epsilon()))
368  {
369  s.get_vconst_curve(bc, vmax);
370  dd=eli::geom::intersect::minimum_distance(uu, bc, pt, u);
371 
372  if (dd<dist)
373  {
374  u=uu;
375  v=vmax;
376  dist=dd;
377  }
378  }
379 
380  return dist;
381 
382  }
383 
384 // Defined for minimum_distance_curve. Could be moved to util somewhere.
385 // template< typename first__, typename second__>
386 // bool pairfirstcompare( const std::pair < first__, second__ > &a, const std::pair < first__, second__ > &b )
387 // {
388 // return ( a.first < b.first );
389 // }
390 
391  template<template<typename, unsigned short, typename> class surface__, typename data__, unsigned short dim__, typename tol__ >
397  {
399  typedef typename piecewise_type::surface_type surface_type;
400  typedef typename piecewise_type::index_type index_type;
401  typedef typename piecewise_type::data_type data_type;
402  typedef typename piecewise_type::bounding_box_type bounding_box_type;
403 
404  typedef typename piecewise_type::keymap_type keymap_type;
405  typedef typename keymap_type::const_iterator keyit;
406 
407  typedef std::pair<keyit, keyit> itpair;
408  typedef std::vector< std::pair<data_type, itpair > > dvec;
409  dvec minbbdist;
410 
411  // Find closest corner of bounding boxes, add them to vector
412  // Simple linear search, would be more efficient with some sort of tree.
413  for(keyit uit = ps.ukey.key.begin(); uit != ps.ukey.key.end(); ++uit)
414  {
415  for(keyit vit = ps.vkey.key.begin(); vit != ps.vkey.key.end(); ++vit)
416  {
417  index_type uk = uit->second;
418  index_type vk = vit->second;
419 
420  surface_type s = ps.patches[uk][vk];
421 
422  bounding_box_type bb_local;
423  s.get_bounding_box(bb_local);
424 
425  data_type dbbmin;
426  dbbmin = minimum_distance(bb_local, pt);
427 
428  minbbdist.push_back(std::make_pair(dbbmin, std::make_pair(uit, vit)));
429 
430  }
431  }
432 
433  // Sort by nearest distance.
434  std::sort( minbbdist.begin(), minbbdist.end(), pairfirstcompare<data_type, itpair > );
435 
436 
437  // Iterate over segments, starting with nearest bounding box
438  data_type dist(std::numeric_limits<data_type>::max());
439 
440  typename dvec::const_iterator it;
441  for (it=minbbdist.begin(); it!=minbbdist.end(); ++it)
442  {
443  // If nearest bb distance is farther than current best, we're done.
444  if(it->first < dist )
445  {
446  itpair itp = it->second;
447  keyit uit = itp.first;
448  keyit vit = itp.second;
449 
450  index_type uk = uit->second;
451  index_type vk = vit->second;
452 
453  surface_type s = ps.patches[uk][vk];
454 
455  data_type uu, vv, d;
456  d=minimum_distance(uu, vv, s, pt);
457 
458  if(d < dist)
459  {
460  data_type du(ps.ukey.get_delta_parm(uit));
461  data_type dv(ps.vkey.get_delta_parm(vit));
462 
463  data_type ustart(uit->first);
464  data_type vstart(vit->first);
465 
466  dist = d;
467  u=ustart+uu*du;
468  v=vstart+vv*dv;
469  }
470  }
471  else
472  {
473  break;
474  }
475 
476  }
477  return dist;
478  }
479 
480  }
481  }
482 }
483 #endif
surface_type::point_type point_type
Definition: piecewise.hpp:59
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
Eigen::Matrix< typename surface__::data_type, 2, 1 > vec
Definition: minimum_distance_surface.hpp:95
surface__::point_type pt
Definition: minimum_distance_surface.hpp:41
const surface__ * ps
Definition: minimum_distance_surface.hpp:40
data__ data_type
Definition: piecewise.hpp:66
Definition: newton_raphson_constrained_system_method.hpp:30
Eigen::Matrix< typename surface__::data_type, 2, 2 > mat
Definition: minimum_distance_surface.hpp:96
Definition: piecewise.hpp:37
const surface__ * ps
Definition: minimum_distance_surface.hpp:93
parameter_key ukey
Definition: piecewise.hpp:1495
Eigen::Matrix< typename surface__::data_type, 2, 1 > vec
Definition: minimum_distance_surface.hpp:42
surface__::point_type pt
Definition: minimum_distance_surface.hpp:94
mat operator()(const vec &u) const
Definition: minimum_distance_surface.hpp:98
patch_collection_type patches
Definition: piecewise.hpp:1492
vec operator()(const vec &u) const
Definition: minimum_distance_surface.hpp:44
parameter_key vkey
Definition: piecewise.hpp:1495
Definition: minimum_distance_surface.hpp:38
Definition: minimum_distance_surface.hpp:91