core/vgl/vgl_distance.txx
Go to the documentation of this file.
00001 // This is core/vgl/vgl_distance.txx
00002 #ifndef vgl_distance_txx_
00003 #define vgl_distance_txx_
00004 //:
00005 // \file
00006 // \author fsm
00007 
00008 #include "vgl_distance.h"
00009 #include <vgl/vgl_point_2d.h>
00010 #include <vgl/vgl_point_3d.h>
00011 #include <vgl/vgl_homg_point_1d.h>
00012 #include <vgl/vgl_homg_point_2d.h>
00013 #include <vgl/vgl_homg_point_3d.h>
00014 #include <vgl/vgl_line_2d.h>
00015 #include <vgl/vgl_line_segment_2d.h>
00016 #include <vgl/vgl_line_segment_3d.h>
00017 #include <vgl/vgl_homg_line_2d.h>
00018 #include <vgl/vgl_homg_line_3d_2_points.h>
00019 #include <vgl/vgl_plane_3d.h>
00020 #include <vgl/vgl_homg_plane_3d.h>
00021 #include <vgl/vgl_polygon.h>
00022 #include <vgl/vgl_closest_point.h>
00023 #include <vcl_cassert.h>
00024 #include <vcl_cmath.h> // for vcl_sqrt()
00025 #include <vcl_utility.h> // for vcl_pair<T,U>
00026 
00027 template <class T>
00028 static inline T square_of(T x) { return x*x; }
00029 
00030 template <class T>
00031 double vgl_distance_to_linesegment(T x1, T y1,
00032                                    T x2, T y2,
00033                                    T x, T y)
00034 {
00035   return vcl_sqrt(vgl_distance2_to_linesegment(x1, y1, x2, y2, x, y));
00036 }
00037 
00038 template <class T>
00039 double vgl_distance2_to_linesegment(T x1, T y1,
00040                                     T x2, T y2,
00041                                     T x, T y)
00042 {
00043   // squared distance between endpoints :
00044   T ddh = square_of(x2-x1) + square_of(y2-y1);
00045 
00046   // squared distance to endpoints :
00047   T dd1 = square_of(x-x1) + square_of(y-y1);
00048   T dd2 = square_of(x-x2) + square_of(y-y2);
00049 
00050   // if closest to the start point :
00051   if (dd2 >= ddh + dd1)
00052     return dd1;
00053 
00054   // if closest to the end point :
00055   if (dd1 >= ddh + dd2)
00056     return dd2;
00057 
00058   // squared perpendicular distance to line :
00059   T a = y1-y2;
00060   T b = x2-x1;
00061   T c = x1*y2-x2*y1;
00062   return square_of(a*x + b*y + c)/double(a*a + b*b);
00063 }
00064 
00065 template <class T>
00066 double vgl_distance_to_linesegment(T x1, T y1, T z1,
00067                                    T x2, T y2, T z2,
00068                                    T x, T y, T z)
00069 {
00070   return vcl_sqrt(vgl_distance2_to_linesegment(x1, y1, z1, x2, y2, z2, x, y, z));
00071 }
00072 
00073 template <class T>
00074 double vgl_distance2_to_linesegment(T x1, T y1, T z1,
00075                                     T x2, T y2, T z2,
00076                                     T x, T y, T z)
00077 {
00078   // squared distance between endpoints :
00079   T ddh = square_of(x2-x1) + square_of(y2-y1) + square_of(z2-z1);
00080 
00081   // squared distance to endpoints :
00082   T dd1 = square_of(x-x1) + square_of(y-y1) + square_of(z-z1);
00083   T dd2 = square_of(x-x2) + square_of(y-y2) + square_of(z-z2);
00084 
00085   // if closest to the start point :
00086   if (dd2 >= ddh + dd1)
00087     return dd1;
00088 
00089   // if closest to the end point :
00090   if (dd1 >= ddh + dd2)
00091     return dd2;
00092 
00093   // squared perpendicular distance to line :
00094   // plane through (x,y,z) and orthogonal to the line is a(X-x)+b(Y-y)+c(Z-z)=0
00095   // where (a,b,c) is the direction of the line.
00096   T a = x2-x1, b = y2-y1, c = z2-z1;
00097   // The closest point is then the intersection of this plane with the line.
00098   // This point equals (x1,y1,z1) + lambda * (a,b,c), with this lambda:
00099   double lambda = (a*(x-x1)+b*(y-y1)+c*(z-z1))/double(a*a+b*b+c*c);
00100   // return squared distance:
00101   return square_of(x-x1-lambda*a) + square_of(y-y1-lambda*b) + square_of(z-z1-lambda*c);
00102 }
00103 
00104 template <class T>
00105 double vgl_distance_to_non_closed_polygon(T const px[], T const py[], unsigned n,
00106                                           T x, T y)
00107 {
00108   double dd = -1;
00109   for (unsigned i=0; i+1<n; ++i) {
00110     double nd = vgl_distance_to_linesegment(px[i  ], py[i  ],
00111                                             px[i+1], py[i+1],
00112                                             x, y);
00113     if (dd<0 || nd<dd)
00114       dd = nd;
00115   }
00116   return dd;
00117 }
00118 
00119 template <class T>
00120 double vgl_distance_to_closed_polygon(T const px[], T const py[], unsigned n,
00121                                       T x, T y)
00122 {
00123   double dd = vgl_distance_to_linesegment(px[n-1], py[n-1],
00124                                           px[0  ], py[0  ],
00125                                           x, y);
00126   for (unsigned i=0; i+1<n; ++i) {
00127     double nd = vgl_distance_to_linesegment(px[i  ], py[i  ],
00128                                             px[i+1], py[i+1],
00129                                             x, y);
00130     if (nd<dd)
00131       dd = nd;
00132   }
00133 
00134   return dd;
00135 }
00136 
00137 template <class T>
00138 double vgl_distance_to_non_closed_polygon(T const px[], T const py[], T const pz[], unsigned int n,
00139                                           T x, T y, T z)
00140 {
00141   double dd = -1;
00142   for (unsigned i=0; i+1<n; ++i) {
00143     double nd = vgl_distance_to_linesegment(px[i  ], py[i  ], pz[i  ],
00144                                             px[i+1], py[i+1], pz[i+1],
00145                                             x, y, z);
00146     if (dd<0 || nd<dd)
00147       dd = nd;
00148   }
00149   return dd;
00150 }
00151 
00152 template <class T>
00153 double vgl_distance_to_closed_polygon(T const px[], T const py[], T const pz[], unsigned int n,
00154                                       T x, T y, T z)
00155 {
00156   double dd = vgl_distance_to_linesegment(px[n-1], py[n-1], pz[n-1],
00157                                           px[0  ], py[0  ], pz[0  ],
00158                                           x, y, z);
00159   for (unsigned i=0; i+1<n; ++i) {
00160     double nd = vgl_distance_to_linesegment(px[i  ], py[i  ], pz[i  ],
00161                                             px[i+1], py[i+1], pz[i+1],
00162                                             x, y, z);
00163     if (nd<dd)
00164       dd = nd;
00165   }
00166 
00167   return dd;
00168 }
00169 
00170 template <class T>
00171 double vgl_distance_origin(vgl_homg_line_2d<T> const& l)
00172 {
00173   if (l.c() == 0) return 0.0; // no call to sqrt if not necessary
00174   else return vcl_abs(static_cast<double>(l.c())) / vcl_sqrt(static_cast<double>( l.a()*l.a()+l.b()*l.b() ));
00175 }
00176 
00177 template <class T>
00178 double vgl_distance_origin(vgl_line_2d<T> const& l)
00179 {
00180   if (l.c() == 0) return 0.0; // no call to sqrt if not necessary
00181   else return vcl_abs(static_cast<double>(l.c())) / vcl_sqrt(static_cast<double>( l.a()*l.a()+l.b()*l.b() ));
00182 }
00183 
00184 template <class T>
00185 double vgl_distance_origin(vgl_homg_plane_3d<T> const& pl)
00186 {
00187   if (pl.d() == 0) return 0.0; // no call to sqrt if not necessary
00188   else return vcl_abs(static_cast<double>(pl.d())) / vcl_sqrt(static_cast<double>( pl.a()*pl.a()+pl.b()*pl.b()+pl.c()*pl.c()) );
00189 }
00190 
00191 template <class T>
00192 double vgl_distance_origin(vgl_plane_3d<T> const& pl)
00193 {
00194   if (pl.d() == 0) return 0.0; // no call to sqrt if not necessary
00195   else return vcl_abs(static_cast<double>(pl.d())) / vcl_sqrt(static_cast<double>( pl.a()*pl.a()+pl.b()*pl.b()+pl.c()*pl.c()) );
00196 }
00197 
00198 template <class T>
00199 double vgl_distance_origin(vgl_homg_line_3d_2_points<T> const& l)
00200 {
00201   vgl_homg_point_3d<T> q = vgl_closest_point_origin(l);
00202   return vcl_sqrt(static_cast<double>(square_of(q.x())+square_of(q.y())+square_of(q.z())))/q.w();
00203 }
00204 
00205 template <class T>
00206 double vgl_distance_origin(vgl_line_3d_2_points<T> const& l)
00207 {
00208   vgl_point_3d<T> q = vgl_closest_point_origin(l);
00209   return vcl_sqrt(static_cast<double>(square_of(q.x())+square_of(q.y())+square_of(q.z())));
00210 }
00211 
00212 template <class T>
00213 double vgl_distance(vgl_homg_point_1d<T>const& p1,
00214                     vgl_homg_point_1d<T>const& p2)
00215 {
00216   return vcl_abs(static_cast<double>(p1.x()/p1.w() - p2.x()/p2.w()));
00217 }
00218 
00219 template <class T>
00220 double vgl_distance(vgl_line_2d<T> const& l, vgl_point_2d<T> const& p)
00221 {
00222   T num = l.a()*p.x() + l.b()*p.y() + l.c();
00223   if (num == 0) return 0.0; // no call to sqrt if not necessary
00224   else return vcl_abs(static_cast<double>(num)) / vcl_sqrt(static_cast<double>(l.a()*l.a() + l.b()*l.b()));
00225 }
00226 
00227 template <class T>
00228 double vgl_distance(vgl_homg_line_2d<T> const& l, vgl_homg_point_2d<T> const& p)
00229 {
00230   T num = l.a()*p.x() + l.b()*p.y() + l.c()*p.w();
00231   if (num == 0) return 0.0; // always return 0 when point on line, even at infinity
00232   else return vcl_abs(static_cast<double>(num)) / vcl_sqrt(static_cast<double>(l.a()*l.a() + l.b()*l.b())) / p.w(); // could be inf
00233 }
00234 
00235 template <class T>
00236 double vgl_distance(vgl_plane_3d<T> const& l, vgl_point_3d<T> const& p)
00237 {
00238   T num = l.nx()*p.x() + l.ny()*p.y() + l.nz()*p.z() + l.d();
00239   if (num == 0) return 0.0; // no call to sqrt if not necessary
00240   else return vcl_abs(static_cast<double>(num)) / vcl_sqrt(static_cast<double>(l.nx()*l.nx() + l.ny()*l.ny() + l.nz()*l.nz()));
00241 }
00242 
00243 template <class T>
00244 double vgl_distance(vgl_homg_plane_3d<T> const& l, vgl_homg_point_3d<T> const& p)
00245 {
00246   T num = l.nx()*p.x() + l.ny()*p.y() + l.nz()*p.z() + l.d()*p.w();
00247   if (num == 0) return 0.0; // always return 0 when point on plane, even at infinity
00248   else return vcl_abs(static_cast<double>(num/p.w())) / vcl_sqrt(static_cast<double>(l.nx()*l.nx() + l.ny()*l.ny() + l.nz()*l.nz()));
00249 }
00250 
00251 template <class T>
00252 double vgl_distance(vgl_polygon<T> const& poly, vgl_point_2d<T> const& point, bool closed)
00253 {
00254   double dist = -1;
00255   for ( unsigned int s=0; s < poly.num_sheets(); ++s )
00256   {
00257     const vcl_vector<vgl_point_2d<T> > &sheet = poly[s];
00258     unsigned int n = (unsigned int)(sheet.size());
00259     assert( n > 1 );
00260     double dd = closed ?
00261                 vgl_distance_to_linesegment(sheet[n-1].x(), sheet[n-1].y(),
00262                                             sheet[0  ].x(), sheet[0  ].y(),
00263                                             point.x(), point.y()) :
00264                 vgl_distance_to_linesegment(sheet[0  ].x(), sheet[0  ].y(),
00265                                             sheet[1  ].x(), sheet[1  ].y(),
00266                                             point.x(), point.y());
00267     for ( unsigned int i=0; i+1 < n; ++i )
00268     {
00269       double nd = vgl_distance_to_linesegment(sheet[i  ].x(), sheet[i  ].y(),
00270                                               sheet[i+1].x(), sheet[i+1].y(),
00271                                               point.x(), point.y());
00272       if ( nd<dd )  dd=nd;
00273     }
00274     if ( dist < 0 || dd < dist )  dist = dd;
00275   }
00276 
00277   return dist;
00278 }
00279 
00280 template <class T>
00281 double vgl_distance(vgl_homg_line_3d_2_points<T> const& line1,
00282                     vgl_homg_line_3d_2_points<T> const& line2)
00283 {
00284   vcl_pair<vgl_homg_point_3d<T>, vgl_homg_point_3d<T> > pp =
00285     vgl_closest_points(line1, line2);
00286   if (pp.first.w() != 0)
00287     return vgl_distance(pp.first,pp.second);
00288   else // the two lines are parallel
00289     return vgl_distance(line1.point_finite(), line2);
00290 }
00291 
00292 
00293 template <class T>
00294 double vgl_distance(vgl_homg_line_3d_2_points<T> const& l,
00295                     vgl_homg_point_3d<T> const& p)
00296 {
00297   vgl_homg_point_3d<T> q = vgl_closest_point(l, p);
00298   return vgl_distance(p,q);
00299 }
00300 
00301 template <class T>
00302 double vgl_distance(vgl_line_3d_2_points<T> const& l,
00303                     vgl_point_3d<T> const& p)
00304 {
00305   vgl_point_3d<T> q = vgl_closest_point(l, p);
00306   return vgl_distance(p,q);
00307 }
00308 
00309 template <class T>
00310 double vgl_distance(vgl_line_segment_2d<T> const& l,
00311                     vgl_point_2d<T> const& p)
00312 {
00313   return vgl_distance_to_linesegment(l.point1().x(), l.point1().y(),
00314                                      l.point2().x(), l.point2().y(),
00315                                      p.x(), p.y());
00316 }
00317 
00318 template <class T>
00319 double vgl_distance(vgl_line_segment_3d<T> const& l,
00320                     vgl_point_3d<T> const& p)
00321 {
00322   return vgl_distance_to_linesegment(l.point1().x(), l.point1().y(), l.point1().z(),
00323                                      l.point2().x(), l.point2().y(), l.point2().z(),
00324                                      p.x(), p.y(), p.z());
00325 }
00326 
00327 //: return the closest distance from a point to a ray
00328 template <class T>
00329 double vgl_distance(vgl_ray_3d<T> const& r, vgl_point_3d<T> const& p)
00330 {
00331   vgl_point_3d<T> q = vgl_closest_point(r, p);
00332   return vgl_distance(p,q);
00333 }
00334 
00335 #undef VGL_DISTANCE_INSTANTIATE
00336 #define VGL_DISTANCE_INSTANTIATE(T) \
00337 template double vgl_distance2_to_linesegment(T,T,T,T,T,T); \
00338 template double vgl_distance_to_linesegment(T,T,T,T,T,T); \
00339 template double vgl_distance2_to_linesegment(T,T,T,T,T,T,T,T,T); \
00340 template double vgl_distance_to_linesegment(T,T,T,T,T,T,T,T,T); \
00341 template double vgl_distance_to_non_closed_polygon(T const[],T const[],unsigned int,T,T); \
00342 template double vgl_distance_to_non_closed_polygon(T const[],T const[],T const[],unsigned int,T,T,T); \
00343 template double vgl_distance_to_closed_polygon(T const[],T const[],unsigned int,T,T); \
00344 template double vgl_distance_to_closed_polygon(T const[],T const[],T const[],unsigned int,T,T,T); \
00345 template double vgl_distance_origin(vgl_line_2d<T >const& l); \
00346 template double vgl_distance_origin(vgl_homg_line_2d<T >const& l); \
00347 template double vgl_distance_origin(vgl_plane_3d<T > const& pl); \
00348 template double vgl_distance_origin(vgl_homg_plane_3d<T > const& pl); \
00349 template double vgl_distance_origin(vgl_line_3d_2_points<T > const& l); \
00350 template double vgl_distance_origin(vgl_homg_line_3d_2_points<T > const& l); \
00351 template double vgl_distance(vgl_homg_point_1d<T >const&,vgl_homg_point_1d<T >const&); \
00352 template double vgl_distance(vgl_point_2d<T >const&,vgl_point_2d<T >const&); \
00353 template double vgl_distance(vgl_homg_point_2d<T >const&,vgl_homg_point_2d<T >const&); \
00354 template double vgl_distance(vgl_point_3d<T >const&,vgl_point_3d<T >const&); \
00355 template double vgl_distance(vgl_homg_point_3d<T >const&,vgl_homg_point_3d<T >const&); \
00356 template double vgl_distance(vgl_line_2d<T >const&,vgl_point_2d<T >const&); \
00357 template double vgl_distance(vgl_point_2d<T >const&,vgl_line_2d<T >const&); \
00358 template double vgl_distance(vgl_homg_line_2d<T >const&,vgl_homg_point_2d<T >const&); \
00359 template double vgl_distance(vgl_homg_point_2d<T >const&,vgl_homg_line_2d<T >const&); \
00360 template double vgl_distance(vgl_plane_3d<T >const&,vgl_point_3d<T >const&); \
00361 template double vgl_distance(vgl_point_3d<T >const&,vgl_plane_3d<T >const&); \
00362 template double vgl_distance(vgl_homg_plane_3d<T >const&,vgl_homg_point_3d<T >const&); \
00363 template double vgl_distance(vgl_homg_point_3d<T >const&,vgl_homg_plane_3d<T >const&); \
00364 template double vgl_distance(vgl_polygon<T >const&,vgl_point_2d<T >const&,bool); \
00365 template double vgl_distance(vgl_homg_line_3d_2_points<T >const&,vgl_homg_line_3d_2_points<T >const&); \
00366 template double vgl_distance(vgl_homg_line_3d_2_points<T >const&,vgl_homg_point_3d<T >const&); \
00367 template double vgl_distance(vgl_line_3d_2_points<T >const&,vgl_point_3d<T >const&); \
00368 template double vgl_distance(vgl_ray_3d<T > const& r, vgl_point_3d<T > const& p); \
00369 template double vgl_distance(vgl_homg_point_3d<T > const&,vgl_homg_line_3d_2_points<T > const&); \
00370 template double vgl_distance(vgl_line_segment_2d<T > const&, vgl_point_2d<T > const&); \
00371 template double vgl_distance(vgl_line_segment_3d<T > const&, vgl_point_3d<T > const&)
00372 
00373 #endif // vgl_distance_txx_
00374