00001
00002 #ifndef vgl_distance_txx_
00003 #define vgl_distance_txx_
00004
00005
00006
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>
00025 #include <vcl_utility.h>
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
00044 T ddh = square_of(x2-x1) + square_of(y2-y1);
00045
00046
00047 T dd1 = square_of(x-x1) + square_of(y-y1);
00048 T dd2 = square_of(x-x2) + square_of(y-y2);
00049
00050
00051 if (dd2 >= ddh + dd1)
00052 return dd1;
00053
00054
00055 if (dd1 >= ddh + dd2)
00056 return dd2;
00057
00058
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
00079 T ddh = square_of(x2-x1) + square_of(y2-y1) + square_of(z2-z1);
00080
00081
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
00086 if (dd2 >= ddh + dd1)
00087 return dd1;
00088
00089
00090 if (dd1 >= ddh + dd2)
00091 return dd2;
00092
00093
00094
00095
00096 T a = x2-x1, b = y2-y1, c = z2-z1;
00097
00098
00099 double lambda = (a*(x-x1)+b*(y-y1)+c*(z-z1))/double(a*a+b*b+c*c);
00100
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;
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;
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;
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;
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;
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;
00232 else return vcl_abs(static_cast<double>(num)) / vcl_sqrt(static_cast<double>(l.a()*l.a() + l.b()*l.b())) / p.w();
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;
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;
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
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
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