1 // This is core/vgl/vgl_closest_point.hxx
2 #ifndef vgl_closest_point_hxx_
3 #define vgl_closest_point_hxx_
4 #include <limits>
5 #include <cmath>
6 #ifdef _MSC_VER
7 #  include <vcl_msvc_warnings.h>
8 #endif
9 //:
10 // \file
11 // \author Peter Vanroose, KULeuven, ESAT/PSI
12 
13 #include "vgl_closest_point.h"
14 #include "vgl_distance.h"
15 #include "vgl_line_2d.h"
16 #include "vgl_homg_line_2d.h"
17 #include "vgl_point_2d.h"
18 #include "vgl_point_3d.h"
19 #include "vgl_vector_3d.h"
20 #include "vgl_homg_point_2d.h"
21 #include "vgl_homg_point_3d.h"
22 #include "vgl_plane_3d.h"
23 #include "vgl_sphere_3d.h"
24 #include "vgl_cylinder_3d.h"
25 #include "vgl_homg_plane_3d.h"
26 #include "vgl_homg_line_3d_2_points.h"
27 #include "vgl_line_3d_2_points.h"
28 #include "vgl_line_segment_2d.h"
29 #include "vgl_line_segment_3d.h"
30 #include "vgl_polygon.h"
31 #include "vgl_ray_3d.h"
32 #include "vgl_pointset_3d.h"
33 #include "vgl_cubic_spline_3d.h"
34 #include <cassert>
35 
36 template <class T>
square(T x)37 static inline T square(T x) { return x*x; }
38 
39 
40 // Consider numbers smaller than this to be zero
41 const double SMALL_DOUBLE = 1e-12;
42 
43 
44 // Borland has trouble deducing template types when parameters have
45 // type const T instead of T. This occurs in
46 // vgl_distance2_to_linesegment. The workaround is to make the T const
47 // parameter a T parameter.
48 //
49 #define DIST_SQR_TO_LINE_SEG_2D( T, x1, y1, x2, y2, x, y ) \
50      vgl_distance2_to_linesegment(x1, y1, x2, y2, x, y );
51 #define DIST_SQR_TO_LINE_SEG_3D( T, x1, y1, z1, x2, y2, z2, x, y, z ) \
52      vgl_distance2_to_linesegment(x1, y1, z1, x2, y2, z2, x, y, z );
53 
54 
55 template <class T>
vgl_closest_point_to_linesegment(T & ret_x,T & ret_y,T x1,T y1,T x2,T y2,T x0,T y0)56 void vgl_closest_point_to_linesegment(T& ret_x, T& ret_y,
57                                       T x1, T y1,
58                                       T x2, T y2,
59                                       T x0, T y0)
60 {
61   // squared distance between endpoints:
62   T ddh = square(x2-x1) + square(y2-y1);
63 
64   // squared distance to endpoints:
65   T dd1 = square(x0-x1) + square(y0-y1);
66   T dd2 = square(x0-x2) + square(y0-y2);
67 
68   // if closest to the start point:
69   if (dd2 > ddh + dd1) { ret_x=x1; ret_y=y1; return; }
70 
71   // if closest to the end point :
72   if (dd1 > ddh + dd2) { ret_x=x2; ret_y=y2; return; }
73 
74   // line through (x0,y0) and perpendicular to the given line is
75   // the line with equation (x-x0)(x2-x1)+(y-y0)(y2-y1)=0.
76   // Then it just remains to intersect these two lines:
77   T dx = x2-x1;
78   T dy = y2-y1;
79   double c = dx*dx+dy*dy;
80   ret_x = T((dx*dx*x0+dy*dy*x1-dx*dy*(y1-y0))/c); // possible rounding error!
81   ret_y = T((dx*dx*y1+dy*dy*y0-dx*dy*(x1-x0))/c);
82 }
83 
84 template <class T>
vgl_closest_point_to_linesegment(T & ret_x,T & ret_y,T & ret_z,T x1,T y1,T z1,T x2,T y2,T z2,T x,T y,T z)85 void vgl_closest_point_to_linesegment(T& ret_x, T& ret_y, T& ret_z,
86                                       T x1, T y1, T z1,
87                                       T x2, T y2, T z2,
88                                       T x, T y, T z)
89 {
90   // squared distance between endpoints:
91   T ddh = square(x2-x1) + square(y2-y1) + square(z2-z1);
92 
93   // squared distance to endpoints :
94   T dd1 = square(x-x1) + square(y-y1) + square(z-z1);
95   T dd2 = square(x-x2) + square(y-y2) + square(z-z2);
96 
97   // if closest to the start point:
98   if (dd2 > ddh + dd1) { ret_x=x1; ret_y=y1; ret_z=z1; return; }
99 
100   // if closest to the end point :
101   if (dd1 > ddh + dd2) { ret_x=x2; ret_y=y2; ret_z=z2; return; }
102 
103   // plane through (x,y,z) and orthogonal to the line is a(X-x)+b(Y-y)+c(Z-z)=0
104   // where (a,b,c) is the direction of the line.
105   T a = x2-x1, b = y2-y1, c = z2-z1;
106   // The closest point is then the intersection of this plane with the line.
107   // This point equals (x1,y1,z1) + lambda * (a,b,c), with this lambda:
108   double lambda = (a*(x-x1)+b*(y-y1)+c*(z-z1))/double(a*a+b*b+c*c);
109   ret_x = x1+T(lambda*a); ret_y = y1+T(lambda*b); ret_z = z1+T(lambda*c);
110 }
111 
112 template <class T>
vgl_closest_point_to_non_closed_polygon(T & ret_x,T & ret_y,T const px[],T const py[],unsigned int n,T x,T y)113 int vgl_closest_point_to_non_closed_polygon(T& ret_x, T& ret_y,
114                                             T const px[], T const py[], unsigned int n,
115                                             T x, T y)
116 {
117   assert(n>1);
118   double dd = DIST_SQR_TO_LINE_SEG_2D(T,px[0],py[0], px[1],py[1], x,y);
119   int di = 0;
120   for (unsigned i=1; i+1<n; ++i)
121   {
122     double nd = DIST_SQR_TO_LINE_SEG_2D(T,px[i],py[i], px[i+1],py[i+1], x,y);
123     if (nd<dd) { dd=nd; di=i; }
124   }
125   vgl_closest_point_to_linesegment(ret_x,ret_y, px[di],py[di], px[di+1],py[di+1], x,y);
126   return di;
127 }
128 
129 template <class T>
vgl_closest_point_to_non_closed_polygon(T & ret_x,T & ret_y,T & ret_z,T const px[],T const py[],T const pz[],unsigned int n,T x,T y,T z)130 int vgl_closest_point_to_non_closed_polygon(T& ret_x, T& ret_y, T& ret_z,
131                                             T const px[], T const py[], T const pz[], unsigned int n,
132                                             T x, T y, T z)
133 {
134   assert(n>1);
135   double dd = DIST_SQR_TO_LINE_SEG_3D(T,px[0],py[0],pz[0], px[1],py[1],pz[1], x,y,z);
136   int di = 0;
137   for (unsigned i=1; i+1<n; ++i)
138   {
139     double nd = DIST_SQR_TO_LINE_SEG_3D(T,px[i],py[i],pz[i], px[i+1],py[i+1],pz[i+1], x,y,z);
140     if (nd<dd) { dd=nd; di=i; }
141   }
142   vgl_closest_point_to_linesegment(ret_x,ret_y,ret_z, px[di],py[di],pz[di],
143                                    px[di+1],py[di+1],pz[di+1], x,y,z);
144   return di;
145 }
146 
147 template <class T>
vgl_closest_point_to_closed_polygon(T & ret_x,T & ret_y,T const px[],T const py[],unsigned int n,T x,T y)148 int vgl_closest_point_to_closed_polygon(T& ret_x, T& ret_y,
149                                         T const px[], T const py[], unsigned int n,
150                                         T x, T y)
151 {
152   assert(n>1);
153   double dd = DIST_SQR_TO_LINE_SEG_2D(T,px[0],py[0], px[n-1],py[n-1], x,y);
154   int di = -1;
155   for (unsigned i=0; i+1<n; ++i)
156   {
157     double nd = DIST_SQR_TO_LINE_SEG_2D(T,px[i],py[i], px[i+1],py[i+1], x,y);
158     if (nd<dd) { dd=nd; di=i; }
159   }
160   if (di == -1) di+=n,
161     vgl_closest_point_to_linesegment(ret_x,ret_y, px[0],py[0], px[n-1],py[n-1], x,y);
162   else
163     vgl_closest_point_to_linesegment(ret_x,ret_y, px[di],py[di], px[di+1],py[di+1], x,y);
164   return di;
165 }
166 
167 template <class T>
vgl_closest_point_to_closed_polygon(T & ret_x,T & ret_y,T & ret_z,T const px[],T const py[],T const pz[],unsigned int n,T x,T y,T z)168 int vgl_closest_point_to_closed_polygon(T& ret_x, T& ret_y, T& ret_z,
169                                         T const px[], T const py[], T const pz[], unsigned int n,
170                                         T x, T y, T z)
171 {
172   assert(n>1);
173   double dd = DIST_SQR_TO_LINE_SEG_3D(T,px[0],py[0],pz[0], px[n-1],py[n-1],pz[n-1], x,y,z);
174   int di = -1;
175   for (unsigned i=0; i+1<n; ++i)
176   {
177     double nd = DIST_SQR_TO_LINE_SEG_3D(T,px[i],py[i],pz[i], px[i+1],py[i+1],pz[i+1], x,y,z);
178     if (nd<dd) { dd=nd; di=i; }
179   }
180   if (di == -1) di+=n,
181     vgl_closest_point_to_linesegment(ret_x,ret_y,ret_z, px[0],py[0],pz[0],
182                                      px[n-1],py[n-1],pz[n-1], x,y,z);
183   else
184     vgl_closest_point_to_linesegment(ret_x,ret_y,ret_z, px[di],py[di],pz[di],
185                                      px[di+1],py[di+1],pz[di+1], x,y,z);
186   return di;
187 }
188 
189 template <class T>
vgl_closest_point_origin(vgl_line_2d<T> const & l)190 vgl_point_2d<T> vgl_closest_point_origin(vgl_line_2d<T> const& l)
191 {
192   T d = l.a()*l.a()+l.b()*l.b();
193   return vgl_point_2d<T>(-l.a()*l.c()/d, -l.b()*l.c()/d);
194 }
195 
196 template <class T>
vgl_closest_point_origin(vgl_homg_line_2d<T> const & l)197 vgl_homg_point_2d<T> vgl_closest_point_origin(vgl_homg_line_2d<T> const& l)
198 {
199   return vgl_homg_point_2d<T>(l.a()*l.c(), l.b()*l.c(),
200                               -l.a()*l.a()-l.b()*l.b());
201 }
202 
203 template <class T>
vgl_closest_point_origin(vgl_plane_3d<T> const & pl)204 vgl_point_3d<T> vgl_closest_point_origin(vgl_plane_3d<T> const& pl)
205 {
206   T d = pl.a()*pl.a()+pl.b()*pl.b()+pl.c()*pl.c();
207   return vgl_point_3d<T>(-pl.a()*pl.d()/d, -pl.b()*pl.d()/d, -pl.c()*pl.d()/d);
208 }
209 
210 template <class T>
vgl_closest_point_origin(vgl_homg_plane_3d<T> const & pl)211 vgl_homg_point_3d<T> vgl_closest_point_origin(vgl_homg_plane_3d<T> const& pl)
212 {
213   return vgl_homg_point_3d<T>(pl.a()*pl.d(), pl.b()*pl.d(), pl.c()*pl.d(),
214                               -pl.a()*pl.a()-pl.b()*pl.b()-pl.c()*pl.c());
215 }
216 
217 template <class T>
vgl_closest_point_origin(vgl_homg_line_3d_2_points<T> const & l)218 vgl_homg_point_3d<T> vgl_closest_point_origin(vgl_homg_line_3d_2_points<T> const& l)
219 {
220   vgl_homg_point_3d<T> const& q = l.point_finite();
221   // The plane through the origin and orthogonal to l is ax+by+cz=0
222   // where (a,b,c,0) is the point at infinity of l.
223   T a = l.point_infinite().x(), b = l.point_infinite().y(), c = l.point_infinite().z(),
224     d = a*a+b*b+c*c;
225   // The closest point is then the intersection of this plane with the line l.
226   // This point equals d * l.point_finite - lambda * l.direction, with lambda:
227   T lambda = a*q.x()+b*q.y()+c*q.z();
228   return vgl_homg_point_3d<T>(d*q.x()-lambda*a*q.w(), d*q.y()-lambda*b*q.w(), d*q.z()-lambda*c*q.w(), d*q.w());
229 }
230 
231 template <class T>
vgl_closest_point_origin(vgl_line_3d_2_points<T> const & l)232 vgl_point_3d<T> vgl_closest_point_origin(vgl_line_3d_2_points<T> const& l)
233 {
234   vgl_point_3d<T> const& q = l.point1();
235   // The plane through the origin and orthogonal to l is ax+by+cz=0
236   // where (a,b,c) is the direction of l.
237   T a = l.point2().x()-q.x(), b = l.point2().y()-q.y(), c = l.point2().z()-q.z(),
238     d = a*a+b*b+c*c;
239   // The closest point is then the intersection of this plane with the line l.
240   // This point equals l.point1 - lambda * l.direction, with lambda:
241   T lambda = (a*q.x()+b*q.y()+c*q.z())/d; // possible rounding error!
242   return vgl_point_3d<T>(q.x()-lambda*a, q.y()-lambda*b, q.z()-lambda*c);
243 }
244 
245 template <class T>
vgl_closest_point(vgl_line_2d<T> const & l,vgl_point_2d<T> const & p)246 vgl_point_2d<T> vgl_closest_point(vgl_line_2d<T> const& l,
247                                   vgl_point_2d<T> const& p)
248 {
249   T d = l.a()*l.a()+l.b()*l.b();
250   assert(d!=0); // line should not be the line at infinity
251   return vgl_point_2d<T>((l.b()*l.b()*p.x()-l.a()*l.b()*p.y()-l.a()*l.c())/d,
252                          (l.a()*l.a()*p.y()-l.a()*l.b()*p.x()-l.b()*l.c())/d);
253 }
254 
255 template <class T>
vgl_closest_point(vgl_homg_line_2d<T> const & l,vgl_homg_point_2d<T> const & p)256 vgl_homg_point_2d<T> vgl_closest_point(vgl_homg_line_2d<T> const& l,
257                                        vgl_homg_point_2d<T> const& p)
258 {
259   T d = l.a()*l.a()+l.b()*l.b();
260   assert(d!=0); // line should not be the line at infinity
261   return vgl_homg_point_2d<T>(l.b()*l.b()*p.x()-l.a()*l.b()*p.y()-l.a()*l.c(),
262                               l.a()*l.a()*p.y()-l.a()*l.b()*p.x()-l.b()*l.c(),
263                               d);
264 }
265 
266 template <class T>
vgl_closest_point(vgl_plane_3d<T> const & l,vgl_point_3d<T> const & p)267 vgl_point_3d<T> vgl_closest_point(vgl_plane_3d<T> const& l,
268                                   vgl_point_3d<T> const& p)
269 {
270   // The planes b(x-x0)=a(y-y0) and c(x-x0)=a(z-z0) are orthogonal
271   // to ax+by+cz+d=0 and go through the point (x0,y0,z0).
272   // Hence take the intersection of these three planes:
273   T d = l.a()*l.a()+l.b()*l.b()+l.c()*l.c();
274   return vgl_point_3d<T>(((l.b()*l.b()+l.c()*l.c())*p.x()-l.a()*l.b()*p.y()-l.a()*l.c()*p.z()-l.a()*l.d())/d,
275                          ((l.a()*l.a()+l.c()*l.c())*p.y()-l.a()*l.b()*p.x()-l.b()*l.c()*p.z()-l.b()*l.d())/d,
276                             ((l.a()*l.a()+l.b()*l.b())*p.z()-l.a()*l.c()*p.x()-l.b()*l.c()*p.y()-l.c()*l.d())/d);
277 }
278 
279 template <class T>
vgl_closest_point(vgl_homg_plane_3d<T> const & l,vgl_homg_point_3d<T> const & p)280 vgl_homg_point_3d<T> vgl_closest_point(vgl_homg_plane_3d<T> const& l,
281                                        vgl_homg_point_3d<T> const& p)
282 {
283   return vgl_homg_point_3d<T>((l.b()*l.b()+l.c()*l.c())*p.x()-l.a()*l.b()*p.y()-l.a()*l.c()*p.z()-l.a()*l.d(),
284                               (l.a()*l.a()+l.c()*l.c())*p.y()-l.a()*l.b()*p.x()-l.b()*l.c()*p.z()-l.b()*l.d(),
285                               (l.a()*l.a()+l.b()*l.b())*p.z()-l.a()*l.c()*p.x()-l.b()*l.c()*p.y()-l.c()*l.d(),
286                               l.a()*l.a()+l.b()*l.b()+l.c()*l.c());
287 }
288 
289 template <class T>
vgl_closest_point(vgl_polygon<T> const & poly,vgl_point_2d<T> const & point,bool closed)290 vgl_point_2d<T> vgl_closest_point(vgl_polygon<T> const& poly,
291                                   vgl_point_2d<T> const& point,
292                                   bool closed)
293 {
294   T x=point.x(), y=point.y();
295   double dd = DIST_SQR_TO_LINE_SEG_2D(T,poly[0][0].x(),poly[0][0].y(), poly[0][1].x(),poly[0][1].y(), x,y);
296   int si = 0, di = 0;
297   for ( unsigned int s=0; s < poly.num_sheets(); ++s )
298   {
299     unsigned int n = (unsigned int)(poly[s].size());
300     assert( n > 1 );
301     for (unsigned i=0; i+1<n; ++i)
302     {
303       double nd = DIST_SQR_TO_LINE_SEG_2D(T,poly[s][i].x(),poly[s][i].y(), poly[s][i+1].x(),poly[s][i+1].y(), x,y);
304       if (nd<dd) { dd=nd; di=i; si=s; }
305     }
306     if (closed)
307     {
308       double nd = DIST_SQR_TO_LINE_SEG_2D(T,poly[s][0].x(),poly[s][0].y(), poly[s][n-1].x(),poly[s][n-1].y(), x,y);
309       if (nd<dd) { dd=nd; di=-1; si=s; }
310     }
311   }
312   T ret_x, ret_y;
313   unsigned int n = (unsigned int)(poly[si].size());
314   if (di == -1)
315     vgl_closest_point_to_linesegment(ret_x,ret_y, poly[si][0].x(),poly[si][0].y(), poly[si][n-1].x(),poly[si][n-1].y(), x,y);
316   else
317     vgl_closest_point_to_linesegment(ret_x,ret_y, poly[si][di].x(),poly[si][di].y(), poly[si][di+1].x(),poly[si][di+1].y(), x,y);
318   return vgl_point_2d<T>(T(ret_x), T(ret_y));
319 }
320 
321 template <class T>
322 std::pair<vgl_homg_point_3d<T>, vgl_homg_point_3d<T> >
vgl_closest_points(vgl_homg_line_3d_2_points<T> const & line1,vgl_homg_line_3d_2_points<T> const & line2)323 vgl_closest_points(vgl_homg_line_3d_2_points<T> const& line1,
324                    vgl_homg_line_3d_2_points<T> const& line2)
325 {
326   std::pair<vgl_homg_point_3d<T>, vgl_homg_point_3d<T> > ret;
327   // parallel or equal lines
328   if ((line1==line2)||(line1.point_infinite()==line2.point_infinite()))
329   {
330     ret.first = ret.second = line1.point_infinite();
331   }
332   // intersecting lines
333   else if (concurrent(line1, line2))
334   {
335     ret.first = ret.second = intersection(line1, line2);
336   }
337   // neither parallel nor intersecting
338   // this is the Paul Bourke code - suitably modified for vxl
339   else
340   {
341     // direction of the two lines and of a crossing line
342     vgl_homg_point_3d<T> p21 = line1.point_infinite();
343     vgl_homg_point_3d<T> p43 = line2.point_infinite();
344     vgl_vector_3d<T> p13 = line1.point_finite()-line2.point_finite();
345 
346     T d1343 = p13.x() * p43.x() + p13.y() * p43.y() + p13.z() * p43.z();
347     T d4321 = p43.x() * p21.x() + p43.y() * p21.y() + p43.z() * p21.z();
348     T d1321 = p13.x() * p21.x() + p13.y() * p21.y() + p13.z() * p21.z();
349     T d4343 = p43.x() * p43.x() + p43.y() * p43.y() + p43.z() * p43.z();
350     T d2121 = p21.x() * p21.x() + p21.y() * p21.y() + p21.z() * p21.z();
351 
352     T denom = d2121 * d4343 - d4321 * d4321;
353     T numer = d1343 * d4321 - d1321 * d4343;
354 
355     // avoid divisions:
356     //T mua = numer / denom;
357     //T mub = (d1343 + d4321 * mua) / d4343;
358     T mu_n = d1343 * denom + d4321 * numer;
359     T mu_d = d4343 * denom;
360 
361     ret.first.set(denom*line1.point_finite().x()+numer*p21.x(),
362                   denom*line1.point_finite().y()+numer*p21.y(),
363                   denom*line1.point_finite().z()+numer*p21.z(),
364                   denom);
365     ret.second.set(mu_d*line2.point_finite().x()+mu_n*p43.x(),
366                    mu_d*line2.point_finite().y()+mu_n*p43.y(),
367                    mu_d*line2.point_finite().z()+mu_n*p43.z(),
368                    mu_d);
369   }
370   return ret;
371 }
372 
373 template <class T>
vgl_closest_point(vgl_homg_line_3d_2_points<T> const & l,vgl_homg_point_3d<T> const & p)374 vgl_homg_point_3d<T> vgl_closest_point(vgl_homg_line_3d_2_points<T> const& l,
375                                        vgl_homg_point_3d<T> const& p)
376 {
377   // Invalid case: the given point is at infinity:
378   if (p.w() == 0) return l.point_infinite();
379   vgl_homg_point_3d<T> const& q = l.point_finite();
380   vgl_vector_3d<T> v = p-q;
381   // The plane through p and orthogonal to l is a(x-px)+b(y-py)+c(z-pz)=0
382   // where (a,b,c,0) is the point at infinity of l.
383   T a = l.point_infinite().x(), b = l.point_infinite().y(), c = l.point_infinite().z(),
384     d = a*a+b*b+c*c;
385   // The closest point is then the intersection of this plane with the line l.
386   // This point equals d * l.point_finite + lambda * l.direction, with lambda:
387   T lambda = a*v.x()+b*v.y()+c*v.z();
388   return vgl_homg_point_3d<T>(d*q.x()+lambda*a*q.w(), d*q.y()+lambda*b*q.w(), d*q.z()+lambda*c*q.w(), d*q.w());
389 }
390 
391 template <class T>
vgl_closest_point_t(vgl_line_3d_2_points<T> const & l,vgl_point_3d<T> const & p)392 double vgl_closest_point_t(vgl_line_3d_2_points<T> const& l,
393                            vgl_point_3d<T> const& p)
394 {
395   vgl_point_3d<T> const& q = l.point1();
396   vgl_vector_3d<T> v = p-q;
397   // The plane through p and orthogonal to l is a(x-px)+b(y-py)+c(z-pz)=0
398   // where (a,b,c,0) is the direction of l.
399   double a = l.point2().x()-q.x(), b = l.point2().y()-q.y(),
400          c = l.point2().z()-q.z(), d = a*a+b*b+c*c;
401   // The closest point is then the intersection of this plane with the line l.
402   // This point equals l.point1 + lambda * l.direction, with lambda:
403   double lambda = (a*v.x()+b*v.y()+c*v.z())/d;
404   return lambda;
405 }
406 
407 // NB This function could be written in terms of the preceding function vgl_closest_point_t()
408 template <class T>
vgl_closest_point(vgl_line_3d_2_points<T> const & l,vgl_point_3d<T> const & p)409 vgl_point_3d<T> vgl_closest_point(vgl_line_3d_2_points<T> const& l,
410                                   vgl_point_3d<T> const& p)
411 {
412   vgl_point_3d<T> const& q = l.point1();
413   vgl_vector_3d<T> v = p-q;
414   // The plane through p and orthogonal to l is a(x-px)+b(y-py)+c(z-pz)=0
415   // where (a,b,c,0) is the direction of l.
416   T a = l.point2().x()-q.x(), b = l.point2().y()-q.y(), c = l.point2().z()-q.z(),
417     d = a*a+b*b+c*c;
418   // The closest point is then the intersection of this plane with the line l.
419   // This point equals l.point1 + lambda * l.direction, with lambda:
420   T lambda = (a*v.x()+b*v.y()+c*v.z())/d; // possible rounding error!
421   return vgl_point_3d<T>(q.x()+lambda*a, q.y()+lambda*b, q.z()+lambda*c);
422 }
423 
424 //: Return the points of closest approach on 2 3D lines.
425 template <class T>
426 std::pair<vgl_point_3d<T>, vgl_point_3d<T> >
vgl_closest_points(const vgl_line_3d_2_points<T> & l1,const vgl_line_3d_2_points<T> & l2,bool * unique)427 vgl_closest_points(const vgl_line_3d_2_points<T>& l1,
428                    const vgl_line_3d_2_points<T>& l2,
429                    bool* unique/*=0*/)
430 {
431   std::pair<vgl_point_3d<T>, vgl_point_3d<T> > ret;
432 
433   // Get the parametric equation of each line
434   // l1: p(s) = p1 + su;  u = p2 - p1
435   // l2: q(t) = q1 + tv;  v = q2 - q1
436   vgl_vector_3d<T> u = l1.direction();
437   vgl_vector_3d<T> v = l2.direction();
438 
439   // Get a vector w from first point on line1 to first point on line2
440   vgl_vector_3d<T> w = l1.point1() - l2.point1();
441 
442   double a = dot_product(u,u);
443   double b = dot_product(u,v);
444   double c = dot_product(v,v);
445   double d = dot_product(u,w);
446   double e = dot_product(v,w);
447 
448   // Calculate the parameters s,t for the closest point on each line
449   double denom = a*c - b*b; // should always be non-negative
450   if (denom<0.0) denom = 0.0;
451   if (denom>SMALL_DOUBLE)
452   {
453     double s = (b*e - c*d) / denom;
454     double t = (a*e - b*d) / denom;
455 
456     ret.first = l1.point_t(s);
457     ret.second = l2.point_t(t);
458     if (unique) *unique = true;
459   }
460   else
461   {
462     // Lines are parallel or collinear.
463     // Arbitrarily, return the first point on line1 and the closest point on line2.
464     double s = 0.0;
465     double t = (b>c ? d/b : e/c);
466     ret.first = l1.point_t(s);
467     ret.second = l2.point_t(t);
468     if (unique) *unique = false;
469   }
470 
471   return ret;
472 }
473 
474 template <class T>
vgl_closest_point(vgl_point_3d<T> const & p,vgl_ray_3d<T> const & r)475 vgl_point_3d<T> vgl_closest_point(vgl_point_3d<T> const& p,
476                                   vgl_ray_3d<T> const& r)
477 {
478   vgl_line_3d_2_points<T> l2(r.origin(), r.origin()+r.direction());
479   return vgl_closest_point(p,l2);
480 }
481 
482 //: Return the points of closest approach on 2 3D line segments.
483 template <class T>
484 std::pair<vgl_point_3d<T>, vgl_point_3d<T> >
vgl_closest_points(vgl_line_segment_3d<T> const & l1,vgl_line_segment_3d<T> const & l2,bool * unique)485 vgl_closest_points(vgl_line_segment_3d<T> const& l1,
486                    vgl_line_segment_3d<T> const& l2,
487                    bool* unique/*=0*/)
488 {
489   std::pair<vgl_point_3d<T>, vgl_point_3d<T> > ret;
490 
491   // Get the parametric equation of each line
492   // l1: p(s) = p1 + su;  u = p2 - p1
493   // l2: q(t) = q1 + tv;  v = q2 - q1
494   vgl_vector_3d<T> u = l1.direction();
495   vgl_vector_3d<T> v = l2.direction();
496 
497   // Get a vector w from first point on line2 to first point on line1
498   vgl_vector_3d<T> w = l1.point1() - l2.point1();
499 
500   double a = dot_product(u,u); assert(a>0.0);
501   double b = dot_product(u,v);
502   double c = dot_product(v,v); assert(c>0.0);
503   double d = dot_product(u,w);
504   double e = dot_product(v,w);
505 
506   // Calculate the parameters s,t for the closest point on each infinite line
507   double denom = a*c - b*b; // should always be non-negative
508   assert(denom>=0.0);
509 
510   // Check whether the closest points on the infinite lines are also
511   // on the finite line segments.
512   // Consider the square [0,1][0,1] in the plane (s,t).
513   // Closest points (s,t) on the infinite lines may lie outside this square.
514   // Closest points on line segments will then lie on the boundary of this square.
515   // Hence, need to check either 1 or 2 edges of the square.
516   double s_numer;
517   double s_denom = denom;
518   double t_numer;
519   double t_denom = denom;
520 
521   if (denom < SMALL_DOUBLE)
522   {
523     // Lines are parallel or collinear
524     s_numer = 0.0;
525     s_denom = 1.0;
526     t_numer = e;
527     t_denom = c;
528     if (unique) *unique = false; // assume this for now; check below.
529   }
530   else
531   {
532     // Calculate the closest points on the infinite lines
533     s_numer = (b*e - c*d);
534     t_numer = (a*e - b*d);
535     if (s_numer < 0.0)
536     {
537       // If sc<0 then the s=0 edge is a candidate
538       s_numer = 0.0;
539       t_numer = e;
540       t_denom = c;
541     }
542     else if (s_numer > s_denom)
543     {
544       // If sc>1 then the s=1 edge is a candidate
545       s_numer = s_denom;
546       t_numer = e + b;
547       t_denom = c;
548     }
549     if (unique) *unique = true;
550   }
551 
552   if (t_numer < 0.0)
553   {
554     // If tc<0 then the t=0 edge is a candidate
555     t_numer = 0.0;
556 
557     // Recalculate sc for this edge
558     if (-d < 0.0)
559       s_numer = 0.0;
560     else if (-d > a)
561       s_numer = s_denom;
562     else
563     {
564       s_numer = -d;
565       s_denom = a;
566     }
567   }
568   else if (t_numer > t_denom)
569   {
570     // If tc>1 then the t=1 edge is a candidate
571     t_numer = t_denom;
572 
573     // Recalculate sc for this edge
574     if ((-d + b) < 0.0)
575       s_numer = 0.0;
576     else if ((-d + b) > a)
577       s_numer = s_denom;
578     else
579     {
580       s_numer = (-d + b);
581       s_denom = a;
582     }
583   }
584 
585   // Now calculate the required values of (s,t)
586   double s = (std::abs(s_numer) < SMALL_DOUBLE ? 0.0 : s_numer / s_denom);
587   double t = (std::abs(t_numer) < SMALL_DOUBLE ? 0.0 : t_numer / t_denom);
588 
589   // Need to verify whether returned closest points are unique
590   // in the case of parallel/collinear line segments
591   if (unique && ! *unique)
592   {
593     if ((s==0.0 || s==1.0) && (t==0.0 || t==1.0))
594       *unique = true;
595   }
596 
597   ret.first = l1.point_t(s);
598   ret.second = l2.point_t(t);
599 
600   return ret;
601 }
602 
603 template <class T>
vgl_closest_point(vgl_line_segment_2d<T> const & l,vgl_point_2d<T> const & p)604 vgl_point_2d<T> vgl_closest_point(vgl_line_segment_2d<T> const& l,
605                                   vgl_point_2d<T> const& p)
606 {
607   vgl_point_2d<T> q;
608   vgl_closest_point_to_linesegment(q.x(), q.y(),
609                                    l.point1().x(), l.point1().y(),
610                                    l.point2().x(), l.point2().y(),
611                                    p.x(), p.y());
612   return q;
613 }
614 
615 template <class T>
vgl_closest_point(vgl_line_segment_3d<T> const & l,vgl_point_3d<T> const & p)616 vgl_point_3d<T> vgl_closest_point(vgl_line_segment_3d<T> const& l,
617                                   vgl_point_3d<T> const& p)
618 {
619  vgl_point_3d<T> q;
620  vgl_closest_point_to_linesegment(q.x(), q.y(), q.z(),
621                                   l.point1().x(), l.point1().y(), l.point1().z(),
622                                   l.point2().x(), l.point2().y(), l.point2().z(),
623                                   p.x(), p.y(), p.z());
624  return q;
625 }
626 template <class T>
vgl_closest_point(vgl_sphere_3d<T> const & s,vgl_point_3d<T> const & p)627 vgl_point_3d<T> vgl_closest_point(vgl_sphere_3d<T> const& s,
628                                   vgl_point_3d<T> const& p){
629   const vgl_point_3d<T>& c = s.centre();
630   // offset p by sphere center
631   double vx = static_cast<double>(p.x()-c.x()), vy = static_cast<double>(p.y()-c.y()),
632     vz = static_cast<double>(p.z()-c.z());
633   double radius = std::sqrt(vx*vx + vy*vy + vz*vz);
634   T azimuth = static_cast<T>(std::atan2(vy, vx));
635   T elevation = static_cast<T>(std::acos(vz/radius));
636   vgl_point_3d<T> ret;
637   s.spherical_to_cartesian(elevation, azimuth, ret);
638   return ret;
639 }
640 
641 template <class T>
vgl_closest_point(vgl_cylinder_3d<T> const & c,vgl_point_3d<T> const & p)642 vgl_point_3d<T> vgl_closest_point(vgl_cylinder_3d<T> const& c,
643                                   vgl_point_3d<T> const& p){
644   // construct the cylinder axis as a 3-d line
645   T radius = c.radius();
646   const vgl_point_3d<T>& cent = c.center();
647   vgl_vector_3d<T> orient = c.orientation();
648   orient/=orient.length(); // unit vector
649   vgl_point_3d<T> p2 = cent + orient;
650   vgl_line_3d_2_points<T> axis(cent, p2);
651   vgl_point_3d<T> cp = vgl_closest_point(p, axis);
652   // unit radial vector
653   vgl_vector_3d<T> rv = p-cp; rv/=rv.length();
654   // point on cylinder surface
655   vgl_point_3d<T> ret = cp + radius*rv;
656   return ret;
657 }
658 
659 template <class T>
vgl_closest_point(vgl_pointset_3d<T> const & ptset,vgl_point_3d<T> const & p,T dist)660 vgl_point_3d<T> vgl_closest_point(vgl_pointset_3d<T> const& ptset,
661                                   vgl_point_3d<T> const& p, T dist){
662   unsigned n = ptset.npts();
663   if(n == 0)
664     return vgl_point_3d<T>();
665   unsigned iclose = 0;
666   double d_close = 1.0/SMALL_DOUBLE;
667   for(unsigned i = 0; i<n; ++i){
668     vgl_point_3d<T> pi = ptset.p(i);
669     double d = (pi-p).length();
670     if(d<d_close){
671       d_close = d;
672       iclose = i;
673     }
674   }
675   vgl_point_3d<T> pc = ptset.p(iclose);
676   if(!ptset.has_normals())
677     return pc;
678   const vgl_vector_3d<T>& norm = ptset.n(iclose);
679   //otherwise construct the plane and find closest point on that
680   if(std::numeric_limits<T>::is_integer){
681     // closest point can be templated over int so cast to double for plane computations
682     vgl_point_3d<double> pd(static_cast<double>(p.x()), static_cast<double>(p.y()), static_cast<double>(p.z()));
683     vgl_point_3d<double> pcd(static_cast<double>(pc.x()), static_cast<double>(pc.y()), static_cast<double>(pc.z()));
684 
685     vgl_vector_3d<double> normd(static_cast<double>(norm.x()),static_cast<double>(norm.y()),static_cast<double>(norm.z()));
686     vgl_plane_3d<double> pld(normd, pcd);
687     vgl_point_3d<double> pc_planed = vgl_closest_point(pld, pd);
688     T dpd = static_cast<T>((pc_planed-pcd).length());
689     if(dpd>dist)
690       return pc;
691     vgl_point_3d<T> pc_plane_T(static_cast<T>(pc_planed.x()), static_cast<T>(pc_planed.y()), static_cast<T>(pc_planed.z()));
692     return pc_plane_T;
693   }else{
694     vgl_plane_3d<T> pl(norm, pc);
695     vgl_point_3d<T> pc_plane = vgl_closest_point(pl, p);
696     T dp = static_cast<T>((pc_plane-p).length());
697     if(dp>dist)
698       return pc;
699     return pc_plane;
700   }
701 }
702 template <class T>
vgl_closest_point(vgl_cubic_spline_3d<T> const & cspl,vgl_point_3d<T> const & p)703 vgl_point_3d<T> vgl_closest_point(vgl_cubic_spline_3d<T> const& cspl, vgl_point_3d<T> const& p){
704   // find the closest knot
705  std::vector<vgl_point_3d<T> > const& knots = cspl.const_knots();
706   unsigned n = cspl.n_knots();
707   double min_dist = std::numeric_limits<double>::max();
708   unsigned min_index = 0;
709   for(unsigned i = 0; i<n; ++i){
710     double d = vgl_distance(knots[i],p);
711     if(d<min_dist){
712       min_dist  = d;
713       min_index = i;
714     }
715   }
716   unsigned start_index = 0, end_index = 0;
717   // find the interval assuming that the closest knots span the closest point
718   // cases
719   if(min_index == 0){
720     if(cspl.closed()){
721       double dm = vgl_distance(knots[n-2], p);
722       double dp = vgl_distance(knots[1], p);
723       if(dp<dm){
724         start_index = 0;
725         end_index = 1;
726       }else{
727         start_index = n-2;
728         end_index = n-1;
729       }
730     }else{
731       start_index = 0;
732       end_index = 1;
733     }
734   }else if(min_index == n-1){
735     if(cspl.closed()){
736       double dm = vgl_distance(knots[n-2], p);
737       double dp = vgl_distance(knots[1], p);
738       if(dp<dm){
739         start_index = 0;
740         end_index = 1;
741       }else{
742         start_index = n-2;
743         end_index = n-1;
744       }
745     }else{
746       start_index = n-2;
747       end_index = n-1;
748     }
749   }else{
750       double dm = vgl_distance(knots[min_index-1], p);
751       double dp = vgl_distance(knots[min_index+1], p);
752       if(dp<dm){
753         start_index = min_index;
754         end_index = min_index+1;
755       }else{
756         start_index = min_index-1;
757         end_index = min_index;
758       }
759   }
760   // iterate to fine closest point (within 1/1000 of the interval)
761   T sindx = static_cast<T>(start_index), eindx = static_cast<T>(end_index);
762   bool first = true;
763   unsigned iter = 0;
764   double tolerance = vgl_distance(knots[start_index],knots[end_index])/T(1000);
765   T t = (sindx+ eindx)/T(2);
766   vgl_point_3d<T> cp = cspl(t);
767   double dcm = vgl_distance(p, cp), dcm0 = 0.0 ;
768   unsigned max_iter = 100;
769   while(first || ((std::fabs(dcm - dcm0)>tolerance)&&iter<max_iter)){
770     dcm0 = dcm;
771     first = false;
772     T tp = (t+eindx)/T(2), tm = (t+sindx)/T(2);
773     double dp = vgl_distance(p, cspl(tp));
774     double dm = vgl_distance(p, cspl(tm));
775     if(dp<dm){
776       sindx = t;
777       t = tp;
778     }else{
779       eindx = t;
780       t = tm;
781     }
782     cp = cspl(t);
783     dcm = vgl_distance(p,cp);
784     iter++;
785   }
786   if(iter>=max_iter)
787     std::cout << " Warning: exceeded num interations in closest point cubic_spline_3d\n";
788   return cp;
789 }
790 
791 #undef DIST_SQR_TO_LINE_SEG_2D
792 #undef DIST_SQR_TO_LINE_SEG_3D
793 
794 #undef VGL_CLOSEST_POINT_INSTANTIATE
795 #define VGL_CLOSEST_POINT_INSTANTIATE(T) \
796 template void vgl_closest_point_to_linesegment(T&,T&,T,T,T,T,T,T); \
797 template void vgl_closest_point_to_linesegment(T&,T&,T&,T,T,T,T,T,T,T,T,T); \
798 template int vgl_closest_point_to_non_closed_polygon(T&,T&,T const[],T const[],unsigned int,T,T); \
799 template int vgl_closest_point_to_non_closed_polygon(T&,T&,T&,T const[],T const[],T const[],unsigned int,T,T,T); \
800 template int vgl_closest_point_to_closed_polygon(T&,T&,T const[],T const[],unsigned int,T,T); \
801 template int vgl_closest_point_to_closed_polygon(T&,T&,T&,T const[],T const[],T const[],unsigned int,T,T,T); \
802 template vgl_point_2d<T > vgl_closest_point_origin(vgl_line_2d<T >const& l); \
803 template vgl_point_3d<T > vgl_closest_point_origin(vgl_plane_3d<T > const& pl); \
804 template vgl_point_3d<T > vgl_closest_point_origin(vgl_line_3d_2_points<T > const& l); \
805 template vgl_homg_point_2d<T > vgl_closest_point_origin(vgl_homg_line_2d<T >const& l); \
806 template vgl_homg_point_3d<T > vgl_closest_point_origin(vgl_homg_plane_3d<T > const& pl); \
807 template vgl_homg_point_3d<T > vgl_closest_point_origin(vgl_homg_line_3d_2_points<T > const& l); \
808 template vgl_point_2d<T > vgl_closest_point(vgl_line_2d<T >const&,vgl_point_2d<T >const&); \
809 template vgl_point_2d<T > vgl_closest_point(vgl_point_2d<T >const&,vgl_line_2d<T >const&); \
810 template double vgl_closest_point_t(vgl_line_3d_2_points<T >const&,vgl_point_3d<T >const&); \
811 template vgl_point_3d<T > vgl_closest_point(vgl_line_3d_2_points<T >const&,vgl_point_3d<T >const&); \
812 template vgl_point_3d<T > vgl_closest_point(vgl_point_3d<T > const& p,vgl_ray_3d<T > const& r); \
813 template vgl_homg_point_2d<T > vgl_closest_point(vgl_homg_line_2d<T >const&,vgl_homg_point_2d<T >const&); \
814 template vgl_homg_point_2d<T > vgl_closest_point(vgl_homg_point_2d<T >const&,vgl_homg_line_2d<T >const&); \
815 template vgl_point_3d<T > vgl_closest_point(vgl_plane_3d<T >const&,vgl_point_3d<T >const&); \
816 template vgl_point_3d<T > vgl_closest_point(vgl_point_3d<T >const&,vgl_plane_3d<T >const&); \
817 template vgl_homg_point_3d<T > vgl_closest_point(vgl_homg_plane_3d<T >const&,vgl_homg_point_3d<T >const&); \
818 template vgl_homg_point_3d<T > vgl_closest_point(vgl_homg_point_3d<T >const&,vgl_homg_plane_3d<T >const&); \
819 template vgl_point_2d<T > vgl_closest_point(vgl_polygon<T >const&,vgl_point_2d<T >const&,bool); \
820 template std::pair<vgl_homg_point_3d<T >,vgl_homg_point_3d<T > > \
821          vgl_closest_points(vgl_homg_line_3d_2_points<T >const&,vgl_homg_line_3d_2_points<T >const&); \
822 template vgl_homg_point_3d<T > vgl_closest_point(vgl_homg_line_3d_2_points<T >const&,vgl_homg_point_3d<T >const&); \
823 template vgl_homg_point_3d<T > vgl_closest_point(vgl_homg_point_3d<T >const&,vgl_homg_line_3d_2_points<T >const&); \
824 template std::pair<vgl_point_3d<T >,vgl_point_3d<T > > \
825          vgl_closest_points(vgl_line_3d_2_points<T >const&, vgl_line_3d_2_points<T >const&, bool*); \
826 template std::pair<vgl_point_3d<T >,vgl_point_3d<T > > \
827          vgl_closest_points(vgl_line_segment_3d<T >const&, vgl_line_segment_3d<T >const&, bool*); \
828 template vgl_point_2d<T > vgl_closest_point(vgl_line_segment_2d<T > const&, vgl_point_2d<T > const&); \
829 template vgl_point_3d<T > vgl_closest_point(vgl_line_segment_3d<T > const&, vgl_point_3d<T > const&); \
830 template vgl_point_3d<T> vgl_closest_point(vgl_sphere_3d<T> const& , vgl_point_3d<T> const& ); \
831 template vgl_point_3d<T> vgl_closest_point(vgl_point_3d<T> const& , vgl_sphere_3d<T> const& ); \
832 template vgl_point_3d<T> vgl_closest_point(vgl_cylinder_3d<T> const& , vgl_point_3d<T> const& ); \
833 template vgl_point_3d<T> vgl_closest_point(vgl_point_3d<T> const& , vgl_cylinder_3d<T> const& ); \
834 template vgl_point_3d<T> vgl_closest_point(vgl_pointset_3d<T> const& ptset, vgl_point_3d<T> const& p, T dist); \
835 template vgl_point_3d<T> vgl_closest_point(vgl_cubic_spline_3d<T> const& cspl, vgl_point_3d<T> const& p)
836 #endif // vgl_closest_point_hxx_
837