1 // This is core/vgl/vgl_intersection.hxx
2 #ifndef vgl_intersection_hxx_
3 #define vgl_intersection_hxx_
4 //:
5 // \file
6 // \author Gamze Tunali
7 
8 #include <limits>
9 #include <cmath>
10 #include <vector>
11 #include "vgl_intersection.h"
12 
13 #ifdef _MSC_VER
14 #  include <vcl_msvc_warnings.h>
15 #endif
16 #include <cassert>
17 #include "vgl_point_2d.h"
18 #include "vgl_line_2d.h"
19 #include "vgl_line_3d_2_points.h"
20 #include "vgl_line_segment_2d.h"
21 #include "vgl_line_segment_3d.h"
22 #include "vgl_ray_3d.h"
23 #include "vgl_vector_3d.h"
24 #include "vgl_box_2d.h"
25 #include "vgl_box_3d.h"
26 #include "vgl_polygon.h"
27 #include "vgl_plane_3d.h"
28 #include "vgl_distance.h"
29 #include "vgl_tolerance.h"
30 #include "vgl_closest_point.h"
31 #include <vgl/vgl_lineseg_test.hxx>
32 
33 static double eps = 1.0e-8; // tolerance for intersections
vgl_near_zero(double x)34 inline bool vgl_near_zero(double x) { return x < eps && x > -eps; }
vgl_near_eq(double x,double y)35 inline bool vgl_near_eq(double x, double y) { return vgl_near_zero(x-y); }
36 
37 //: Return the intersection of two boxes (which is itself is a box, possibly the empty box)
38 // \relatesalso vgl_box_2d
39 template <class T>
vgl_intersection(vgl_box_2d<T> const & b1,vgl_box_2d<T> const & b2)40 vgl_box_2d<T> vgl_intersection(vgl_box_2d<T> const& b1,vgl_box_2d<T> const& b2)
41 {
42   T xmin = b1.min_x() > b2.min_x() ? b1.min_x() : b2.min_x();
43   T ymin = b1.min_y() > b2.min_y() ? b1.min_y() : b2.min_y();
44   T xmax = b1.max_x() < b2.max_x() ? b1.max_x() : b2.max_x();
45   T ymax = b1.max_y() < b2.max_y() ? b1.max_y() : b2.max_y();
46   return vgl_box_2d<T>(xmin,xmax,ymin,ymax);
47 }
48 
49 //: Return true if line intersects box. If so, compute intersection points.
50 // \relatesalso vgl_box_3d
51 // \relatesalso vgl_infinite_line_3d
52 template <class T>
vgl_intersection(vgl_box_3d<T> const & box,vgl_infinite_line_3d<T> const & line_3d,vgl_point_3d<T> & p0,vgl_point_3d<T> & p1)53 bool vgl_intersection(vgl_box_3d<T> const& box,
54                       vgl_infinite_line_3d<T> const& line_3d,
55                       vgl_point_3d<T>& p0,
56                       vgl_point_3d<T>& p1)
57 {
58   vgl_point_3d<T> lpt = line_3d.point();
59   vgl_vector_3d<T> di = line_3d.direction();
60   vgl_point_3d<double> dpt(static_cast<double>(lpt.x()),
61                            static_cast<double>(lpt.y()),
62                            static_cast<double>(lpt.z()));
63   vgl_vector_3d<double> dir(static_cast<double>(di.x()),
64                             static_cast<double>(di.y()),
65                             static_cast<double>(di.z()));
66   vgl_infinite_line_3d<double> dline_3d(dpt, dir);
67   // expand box by epsilon tolerance
68   double xmin = box.min_x(), xmax = box.max_x(),
69          ymin = box.min_y(), ymax = box.max_y(),
70          zmin = box.min_z(), zmax = box.max_z();
71   vgl_point_3d<double> minp(xmin, ymin, zmin), maxp(xmax, ymax, zmax);
72   // find intersection point of the line with each of the six box planes
73   vgl_vector_3d<double> vxmin(-1.0, 0.0, 0.0), vxmax(1.0, 0.0, 0.0),
74                         vymin(0.0, -1.0, 0.0), vymax(0.0, 1.0, 0.0),
75                         vzmin(0.0, 0.0, -1.0), vzmax(0.0, 0.0, 1.0);
76   vgl_plane_3d<double> pl_xmin(vxmin, minp),
77                        pl_xmax(vxmax, maxp),
78                        pl_ymin(vymin, minp),
79                        pl_ymax(vymax, maxp),
80                        pl_zmin(vzmin, minp),
81                        pl_zmax(vzmax, maxp);
82   vgl_point_3d<double> pt_xmin(.0,.0,.0), pt_xmax(.0,.0,.0), // dummy initializ.
83                        pt_ymin(.0,.0,.0), pt_ymax(.0,.0,.0), // to avoid
84                        pt_zmin(.0,.0,.0), pt_zmax(.0,.0,.0); // compiler warning
85   bool xmin_good = vgl_intersection(dline_3d, pl_xmin, pt_xmin);
86   bool xmax_good = vgl_intersection(dline_3d, pl_xmax, pt_xmax);
87   bool ymin_good = vgl_intersection(dline_3d, pl_ymin, pt_ymin);
88   bool ymax_good = vgl_intersection(dline_3d, pl_ymax, pt_ymax);
89   bool zmin_good = vgl_intersection(dline_3d, pl_zmin, pt_zmin);
90   bool zmax_good = vgl_intersection(dline_3d, pl_zmax, pt_zmax);
91   // Go through the six cases and return the two intersection points
92   // that lie on box faces. Find the pair that are farthest apart.
93   // There could be multiple intersections if the line passes through the
94   // corners of the box.
95   unsigned int npts = 0;
96   vgl_point_3d<double> dp0=pt_xmin, dp1=pt_xmax; // keep this initialisation!
97 
98   // y-z face at xmin
99   if (xmin_good &&
100       pt_xmin.y()>=ymin && pt_xmin.y()<=ymax &&
101       pt_xmin.z()>=zmin && pt_xmin.z()<=zmax)
102   {
103     // dp0 = pt_xmin; // not needed: already set when dp0 was initialised
104     ++npts;
105   }
106   // y-z face at xmax
107   if (xmax_good &&
108       pt_xmax.y()>=ymin && pt_xmax.y()<=ymax &&
109       pt_xmax.z()>=zmin && pt_xmax.z()<=zmax)
110   {
111     if  (npts == 0) dp0 = pt_xmax;
112     // else         dp1 = pt_xmax; // not needed: already set when dp1 was initialised
113     ++npts;
114   }
115   // x-z face at ymin
116   if (ymin_good &&
117       pt_ymin.x()>=xmin && pt_ymin.x()<=xmax &&
118       pt_ymin.z()>=zmin && pt_ymin.z()<=zmax)
119   {
120     if      (npts == 0) { dp0 = pt_ymin; ++npts; }
121     else if (npts == 1) { dp1 = pt_ymin; ++npts; }
122     else /* npts == 2*/ if (sqr_length(pt_ymin-dp0)>sqr_length(dp1-dp0)) dp1 = pt_ymin;
123   }
124   // x-z face at ymax
125   if (ymax_good &&
126       pt_ymax.x()>=xmin && pt_ymax.x()<=xmax &&
127       pt_ymax.z()>=zmin && pt_ymax.z()<=zmax)
128   {
129     if      (npts == 0) { dp0 = pt_ymax; ++npts; }
130     else if (npts == 1) { dp1 = pt_ymax; ++npts; }
131     else /* npts == 2*/ if (sqr_length(pt_ymax-dp0)>sqr_length(dp1-dp0)) dp1 = pt_ymax;
132   }
133   // x-y face at zmin
134   if (zmin_good &&
135       pt_zmin.x()>=xmin && pt_zmin.x()<=xmax &&
136       pt_zmin.y()>=ymin && pt_zmin.y()<=ymax)
137   {
138     if      (npts == 0) { dp0 = pt_zmin; ++npts; }
139     else if (npts == 1) { dp1 = pt_zmin; ++npts; }
140     else /* npts == 2*/ if (sqr_length(pt_zmin-dp0)>sqr_length(dp1-dp0)) dp1 = pt_zmin;
141   }
142 
143   // x-y face at zmax
144   if (zmax_good &&
145       pt_zmax.x()>=xmin && pt_zmax.x()<=xmax &&
146       pt_zmax.y()>=ymin && pt_zmax.y()<=ymax)
147   {
148     if      (npts == 0) { dp0 = pt_zmax; ++npts; }
149     else if (npts == 1) { dp1 = pt_zmax; ++npts; }
150     else /* npts == 2*/ if (sqr_length(pt_zmax-dp0)>sqr_length(dp1-dp0)) dp1 = pt_zmax;
151   }
152 
153   if (npts==2) {
154     p0.set(static_cast<T>(dp0.x()),
155            static_cast<T>(dp0.y()),
156            static_cast<T>(dp0.z()));
157     p1.set(static_cast<T>(dp1.x()),
158            static_cast<T>(dp1.y()),
159            static_cast<T>(dp1.z()));
160     return true;
161   }
162   else
163     return false;
164 }
165 
166 //: Return true if ray intersects box. If so, compute intersection points.
167 // If ray origin is inside box then p0==p1
168 // \relatesalso vgl_box_3d
169 // \relatesalso vgl_ray_3d
170 template <class T>
vgl_intersection(vgl_box_3d<T> const & box,vgl_ray_3d<T> const & ray,vgl_point_3d<T> & p0,vgl_point_3d<T> & p1)171 bool vgl_intersection(vgl_box_3d<T> const& box,
172                       vgl_ray_3d<T> const& ray,
173                       vgl_point_3d<T>& p0,
174                       vgl_point_3d<T>& p1)
175 {
176   // convert ray to infinite line
177   vgl_infinite_line_3d<T> linf(ray.origin(), ray.direction());
178   bool good_inter = vgl_intersection(box, linf, p0, p1);
179   if (!good_inter) return false;
180   // check if ray origin is inside the box
181   vgl_point_3d<T> org = ray.origin();
182   if (!box.contains(org))
183     //check if the intersection points are in the ray domain
184     return ray.contains(p0)&&ray.contains(p1);
185 
186   //ray origin is inside the box so find the intersection point in the
187   //positive ray domain
188   if (ray.contains(p0)) {
189     p1 = p0; return true;
190   }
191   if (ray.contains(p1)) {
192     p0 = p1; return true;
193   }
194   return false;
195 }
196 
197 //: Return true if a box and plane intersect in 3D
198 // \relatesalso vgl_plane_3d
199 // \relatesalso vgl_box_3d
200 template <class T>
vgl_intersection(vgl_box_3d<T> const & b,vgl_plane_3d<T> const & plane)201 bool vgl_intersection(vgl_box_3d<T> const& b, vgl_plane_3d<T> const& plane)
202 {
203   // find the box corners
204   std::vector<vgl_point_3d<T> > corners;
205   corners.push_back(b.min_point());
206   corners.push_back(vgl_point_3d<T> (b.min_x()+b.width(), b.min_y(), b.min_z()));
207   corners.push_back(vgl_point_3d<T> (b.min_x()+b.width(), b.min_y()+b.height(), b.min_z()));
208   corners.push_back(vgl_point_3d<T> (b.min_x(), b.min_y()+b.height(), b.min_z()));
209   corners.push_back(vgl_point_3d<T> (b.min_x(), b.min_y(), b.max_z()));
210   corners.push_back(vgl_point_3d<T> (b.min_x()+b.width(), b.min_y(), b.max_z()));
211   corners.push_back(b.max_point());
212   corners.push_back(vgl_point_3d<T> (b.min_x(), b.min_y()+b.height(), b.max_z()));
213 
214   // find the signed distance from the box corners to the plane
215   int pos=0, neg=0;
216   for (unsigned int c=0; c<corners.size(); c++) {
217     vgl_point_3d<T> corner=corners[c];
218     double d=(plane.a()*corner.x());
219     d+=(plane.b()*corner.y());
220     d+=(plane.c()*corner.z());
221     d+=plane.d();
222     if (d > 0)
223       pos++;
224     else if (d<0)
225       neg++;
226   }
227   return neg!=8 && pos!=8; // completely inside polygon plane
228 }
229 
230 //: Return the intersection of two boxes (which is itself either a box, or empty)
231 // \relatesalso vgl_box_3d
232 template <class T>
vgl_intersection(vgl_box_3d<T> const & b1,vgl_box_3d<T> const & b2)233 vgl_box_3d<T> vgl_intersection(vgl_box_3d<T> const& b1,vgl_box_3d<T> const& b2)
234 {
235   T xmin = b1.min_x() > b2.min_x() ? b1.min_x() : b2.min_x();
236   T ymin = b1.min_y() > b2.min_y() ? b1.min_y() : b2.min_y();
237   T zmin = b1.min_z() > b2.min_z() ? b1.min_z() : b2.min_z();
238   T xmax = b1.max_x() < b2.max_x() ? b1.max_x() : b2.max_x();
239   T ymax = b1.max_y() < b2.max_y() ? b1.max_y() : b2.max_y();
240   T zmax = b1.max_z() < b2.max_z() ? b1.max_z() : b2.max_z();
241   return vgl_box_3d<T>(xmin,ymin,zmin,xmax,ymax,zmax);
242 }
243 
244 //: compute the intersection of an infinite line with *this box.
245 //  p0 and p1 are the intersection points
246 // In the normal case (no degeneracies) there are six possible intersection combinations:
247 // \verbatim
248 //
249 //                C01 /    CY     \ C11
250 //                   /     |       \           .
251 //       ymax  -----/------|--------\-----
252 //            |    /       |         \    |
253 //            |   /        |          \   |
254 //            |  /         |           \  | \  .
255 //            | /          |            \ |  \_ Bounding Box
256 //            |/           |             \|
257 //            /            |              \    .
258 //           /|            |              |\   .
259 //           ---------------------------------- CX
260 //          \ |            |              /
261 //           \|            |             /|
262 //            \            |            / |
263 //            |\           |           /  |
264 //            | \          |          /   |
265 //            |  \         |         /    |
266 //       xmin  ---\--------|--------/-----   xmax
267 //       ymin      \       |       /
268 //              C00 \             / C10
269 // \endverbatim
270 
271 template <class Type>
vgl_intersection(const vgl_box_2d<Type> & box,const vgl_line_2d<Type> & line,vgl_point_2d<Type> & p0,vgl_point_2d<Type> & p1)272 bool vgl_intersection(const vgl_box_2d<Type>& box,
273                       const vgl_line_2d<Type>& line,
274                       vgl_point_2d<Type>& p0,
275                       vgl_point_2d<Type>& p1)
276 {
277   double a = line.a(), b = line.b(), c = line.c();
278   double xmin=box.min_x(), xmax=box.max_x();
279   double ymin=box.min_y(), ymax=box.max_y();
280 
281   // Run through the cases
282   //
283   if (vgl_near_zero(a))// The line is y = -c/b
284   {
285     float y0 = static_cast<float>(-c/b);
286     // The box edge is collinear with line?
287     if (vgl_near_eq(ymin,y0))
288     {
289       p0.set(static_cast<Type>(xmin), static_cast<Type>(ymin));
290       p1.set(static_cast<Type>(xmax), static_cast<Type>(ymin));
291       return true;
292     }
293     if (vgl_near_eq(ymax,y0))
294     {
295       p0.set(static_cast<Type>(xmin), static_cast<Type>(ymax));
296       p1.set(static_cast<Type>(xmax), static_cast<Type>(ymax));
297       return true;
298     }
299 
300     if ((ymin > y0) || (y0 > ymax)) // The line does not intersect the box
301       return false;
302     else // The line does intersect
303     {
304       p0.set(static_cast<Type>(xmin), static_cast<Type>(y0));
305       p1.set(static_cast<Type>(xmax), static_cast<Type>(y0));
306       return true;
307     }
308   }
309 
310   if (vgl_near_zero(b))// The line is x = -c/a
311   {
312     float x0 = static_cast<float>(-c/a);
313     // The box edge is collinear with l?
314     if (vgl_near_eq(xmin,x0))
315     {
316       p0.set(static_cast<Type>(xmin), static_cast<Type>(ymin));
317       p1.set(static_cast<Type>(xmin), static_cast<Type>(ymax));
318       return true;
319     }
320     if (vgl_near_eq(xmax,x0))
321     {
322       p0.set(static_cast<Type>(xmax), static_cast<Type>(ymin));
323       p1.set(static_cast<Type>(xmax), static_cast<Type>(ymax));
324       return true;
325     }
326 
327     if (xmin <= x0 && x0 <= xmax) // The line intersects the box
328     {
329       p0.set(static_cast<Type>(x0), static_cast<Type>(ymin));
330       p1.set(static_cast<Type>(x0), static_cast<Type>(ymax));
331       return true;
332     }
333     else
334       return false;
335   }
336 
337   // The normal case with no degeneracies
338   //
339   // Intersection with x = xmin
340   float y_xmin_int = static_cast<float>(-(c + a*xmin)/b);
341   bool inside_xmin = (y_xmin_int >= ymin) && (y_xmin_int <= ymax);
342 
343   // Intersection with x = xmax
344   float y_xmax_int = static_cast<float>(-(c + a*xmax)/b);
345   bool inside_xmax = (y_xmax_int >= ymin) && (y_xmax_int <= ymax);
346 
347   // Intersection with y = ymin
348   float x_ymin_int = static_cast<float>(-(c + b*ymin)/a);
349   bool inside_ymin = (x_ymin_int >= xmin) && (x_ymin_int <= xmax);
350 
351   // Intersection with y = ymax
352   float x_ymax_int = static_cast<float>(-(c + b*ymax)/a);
353   bool inside_ymax = (x_ymax_int >= xmin) && (x_ymax_int <= xmax);
354 
355   // Case CX
356   if (inside_xmin && inside_xmax &&
357       !(vgl_near_eq(y_xmin_int,ymin) && vgl_near_eq(y_xmax_int,ymax)))
358   {
359     p0.set(static_cast<Type>(xmin), static_cast<Type>(y_xmin_int));
360     p1.set(static_cast<Type>(xmax), static_cast<Type>(y_xmax_int));
361     return true;
362   }
363 
364   // Case CY
365   if (inside_ymin && inside_ymax &&
366       !(vgl_near_eq(x_ymin_int,xmin) && vgl_near_eq(x_ymax_int,xmax)))
367   {
368     p0.set(static_cast<Type>(x_ymin_int), static_cast<Type>(ymin));
369     p1.set(static_cast<Type>(x_ymax_int), static_cast<Type>(ymax));
370     return true;
371   }
372 
373   // Case C00
374   if (inside_xmin && inside_ymin &&
375       !(inside_xmax && inside_ymax))
376   {
377     p0.set(static_cast<Type>(xmin), static_cast<Type>(y_xmin_int));
378     p1.set(static_cast<Type>(x_ymin_int), static_cast<Type>(ymin));
379     return true;
380   }
381 
382   // Case C01
383   if (inside_xmin && inside_ymax &&
384       !(inside_xmax && inside_ymin))
385   {
386     p0.set(static_cast<Type>(xmin), static_cast<Type>(y_xmin_int));
387     p1.set(static_cast<Type>(x_ymax_int), static_cast<Type>(ymax));
388     return true;
389   }
390 
391   // Case C10
392   if (inside_ymin && inside_xmax &&
393       !(inside_xmin && inside_ymax))
394   {
395     p0.set(static_cast<Type>(x_ymin_int), static_cast<Type>(ymin));
396     p1.set(static_cast<Type>(xmax), static_cast<Type>(y_xmax_int));
397     return true;
398   }
399 
400   // Case C11
401   if (inside_ymax && inside_xmax &&
402       !(inside_xmin && inside_ymin))
403   {
404     p0.set(static_cast<Type>(x_ymax_int), static_cast<Type>(ymax));
405     p1.set(static_cast<Type>(xmax), static_cast<Type>(y_xmax_int));
406     return true;
407   }
408   // Exactly passing through diagonal of BB
409   if (inside_xmin && inside_xmax && inside_ymin && inside_ymax)
410   {
411     if (a>0) // 45 degrees
412     {
413       p0.set(static_cast<Type>(xmin), static_cast<Type>(ymin));
414       p1.set(static_cast<Type>(xmax), static_cast<Type>(ymax));
415       return true;
416     }
417     else // 135 degrees
418     {
419       p0.set(static_cast<Type>(xmin), static_cast<Type>(ymax));
420       p1.set(static_cast<Type>(xmax), static_cast<Type>(ymin));
421       return true;
422     }
423   }
424   return false;
425 }
426 
427 //: Returns the number of intersections of a line segment with a box, up to two are returned in p0 and p1.
428 template <class Type>
vgl_intersection(const vgl_box_2d<Type> & box,const vgl_line_segment_2d<Type> & line_seg,vgl_point_2d<Type> & p0,vgl_point_2d<Type> & p1)429 unsigned int vgl_intersection(const vgl_box_2d<Type>& box,
430                               const vgl_line_segment_2d<Type>& line_seg,
431                               vgl_point_2d<Type>& p0,
432                               vgl_point_2d<Type>& p1)
433 {
434   vgl_line_2d<Type> line(line_seg.a(), line_seg.b(), line_seg.c());
435   vgl_point_2d<Type> pi0, pi1;
436   // if no intersection just return
437   if (!vgl_intersection<Type>(box, line, pi0, pi1))
438     return 0;
439   unsigned int nint = 0;
440   // check if intersection points are interior to the line segment
441   if (vgl_lineseg_test_point<Type>(pi0, line_seg)) {
442     p0 = pi0;
443     nint++;
444   }
445   if (vgl_lineseg_test_point<Type>(pi1, line_seg)) {
446     p1 = pi1;
447     nint++;
448   }
449   return nint;
450 }
451 // return the line segment that lies inside the box. If none, return false.
452 template <class T>
vgl_intersection(vgl_box_2d<T> const & box,vgl_line_segment_2d<T> const & line_seg,vgl_line_segment_2d<T> & int_line_seg)453 bool vgl_intersection(vgl_box_2d<T> const& box,
454                       vgl_line_segment_2d<T> const& line_seg,
455                       vgl_line_segment_2d<T>& int_line_seg){
456   // check if both segment endpoints are inside the box
457   const vgl_point_2d<T>& p1 = line_seg.point1();
458   const vgl_point_2d<T>& p2 = line_seg.point2();
459   bool p1_in_box = box.contains(p1);
460   bool p2_in_box = box.contains(p2);
461   if(p1_in_box&&p2_in_box){
462     int_line_seg = line_seg;
463     return true;
464   }
465   // form an infinite line
466   vgl_line_2d<T> line(line_seg.a(), line_seg.b(), line_seg.c());
467   vgl_point_2d<T> pia, pib;
468   // if no intersection just return
469   if (!vgl_intersection<T>(box, line, pia, pib))
470     return false;
471 
472   // check if intersection points are interior to the line segment
473   bool pia_valid = vgl_lineseg_test_point<T>(pia, line_seg);
474   bool pib_valid = vgl_lineseg_test_point<T>(pib, line_seg);
475   // shouldn't happen but to be complete ...
476   if((!pia_valid)&&(!pib_valid))
477     return false;
478   //if both intersection points are interior to the line segment then
479   //return a segment that spans the box interior
480   if(pia_valid&&pib_valid){
481     int_line_seg.set(pia, pib);
482     return true;
483   }
484   //only one intersection point is valid so form the line segment interior
485   //to the box
486   // find the original segment endpoint inside the box
487   if(p1_in_box){// p1 is inside the box
488     if(pia_valid)
489       int_line_seg.set(p1, pia);
490     else
491       int_line_seg.set(p1, pib);
492     return true;
493   }else{// p2 is inside the box
494     if(pia_valid)
495       int_line_seg.set(p2, pia);
496     else
497       int_line_seg.set(p2, pib);
498     return true;
499   }
500   return false;
501 }
502 template <class T>
vgl_intersection(vgl_line_segment_2d<T> const & line1,vgl_line_segment_2d<T> const & line2,vgl_point_2d<T> & int_pt)503 bool vgl_intersection(vgl_line_segment_2d<T> const& line1,
504                       vgl_line_segment_2d<T> const& line2,
505                       vgl_point_2d<T>& int_pt){
506 	T tol = T(1000)*vgl_tolerance<T>::position;
507   vgl_line_2d<T> l1(line1.point1(), line1.point2());
508   vgl_line_2d<T> l2(line2.point1(), line2.point2());
509   if(!vgl_intersection(l1, l2, int_pt))
510     return false;
511   vgl_vector_2d<T> vs1 = line1.point2() - line1.point1();
512   vgl_vector_2d<T> vs2 = line2.point2() - line2.point1();
513   vgl_vector_2d<T> vp1 = int_pt - line1.point1();
514   vgl_vector_2d<T> vp2 = int_pt - line2.point1();
515   if(dot_product(vs1, vp1)< -tol)
516     return false;
517   if(dot_product(vs2, vp2)< -tol)
518     return false;
519   // check if the intersection point lies inside the
520   // segment boundaries, including the endpoints
521   // require a tolerance due to the inaccuracy of intersection
522 
523   T mag_s1 = vs1.length(), mag_s2 = vs2.length();
524   T mag_p1 = vp1.length(), mag_p2 = vp2.length();
525   T v1 = mag_p1 - mag_s1;
526   T v2 = mag_p2 - mag_s2;
527   if(v1 > tol)
528     return false;
529   if (v2 > tol)
530     return false;
531   return true;
532 }
533 //: Return the intersection point of two concurrent lines
534 template <class T>
vgl_intersection(vgl_line_3d_2_points<T> const & l1,vgl_line_3d_2_points<T> const & l2)535 vgl_point_3d<T> vgl_intersection(vgl_line_3d_2_points<T> const& l1,
536                                  vgl_line_3d_2_points<T> const& l2)
537 {
538   assert(concurrent(l1,l2));
539   T a0=l1.point1().x(),a1=l1.point2().x(),a2=l2.point1().x(),a3=l2.point2().x(),
540     b0=l1.point1().y(),b1=l1.point2().y(),b2=l2.point1().y(),b3=l2.point2().y(),
541     c0=l1.point1().z(),c1=l1.point2().z(),c2=l2.point1().z(),c3=l2.point2().z();
542   T t1 = (b3-b2)*(a1-a0)-(a3-a2)*(b1-b0), t2 = (b0-b2)*(a1-a0)-(a0-a2)*(b1-b0);
543   if (std::abs(t1) < 0.000001)
544   {
545     t1 = (c3-c2)*(a1-a0)-(a3-a2)*(c1-c0), t2 = (c0-c2)*(a1-a0)-(a0-a2)*(c1-c0);
546     if (std::abs(t1) < 0.000001)
547       t1 = (c3-c2)*(b1-b0)-(b3-b2)*(c1-c0), t2 = (c0-c2)*(b1-b0)-(b0-b2)*(c1-c0);
548   }
549 
550   return vgl_point_3d<T>(((t1-t2)*a2+t2*a3)/t1,
551                          ((t1-t2)*b2+t2*b3)/t1,
552                          ((t1-t2)*c2+t2*c3)/t1);
553 }
554 
555 //: Return the intersection point of segments of two concurrent lines
556 // \relatesalso vgl_line_segment_3d
557 template <class T>
vgl_intersection(vgl_line_segment_3d<T> const & l1,vgl_line_segment_3d<T> const & l2,vgl_point_3d<T> & i_pnt)558 bool vgl_intersection(vgl_line_segment_3d<T> const& l1,
559                       vgl_line_segment_3d<T> const& l2,
560                       vgl_point_3d<T>& i_pnt)
561 {
562   vgl_line_3d_2_points<T> l21(l1.point1(),l1.point2());
563   vgl_line_3d_2_points<T> l22(l2.point1(),l2.point2());
564   if (!concurrent(l21, l22))
565     return false;
566   i_pnt = vgl_intersection(l21, l22);
567 
568   double l1_len =   length(l1.point1() - l1.point2());
569   double l1_idist = length(l1.point1() - i_pnt) + length(l1.point2() - i_pnt);
570   double l2_len =   length(l2.point1() - l2.point2());
571   double l2_idist = length(l2.point1() - i_pnt) + length(l2.point2() - i_pnt);
572   return vgl_near_zero(l1_len - l1_idist) && vgl_near_zero(l2_len - l2_idist);
573 }
574 
575 //: Return the intersection point of segments of two concurrent lines
576 // \relatesalso vgl_line_segment_3d
577 // \relatesalso vgl_line_3d_2_points
578 template <class T>
vgl_intersection(vgl_line_3d_2_points<T> const & l1,vgl_line_segment_3d<T> const & l2,vgl_point_3d<T> & i_pnt)579 bool vgl_intersection(vgl_line_3d_2_points<T> const& l1,
580                       vgl_line_segment_3d<T> const& l2,
581                       vgl_point_3d<T>& i_pnt)
582 {
583   vgl_line_3d_2_points<T> l22(l2.point1(),l2.point2());
584   if (!concurrent(l1, l22))
585     return false;
586   i_pnt = vgl_intersection(l1, l22);
587 
588   double l1_len =   length(l1.point1() - l1.point2());
589   double l1_idist = length(l1.point1() - i_pnt) + length(l1.point2() - i_pnt);
590   double l2_len =   length(l2.point1() - l2.point2());
591   double l2_idist = length(l2.point1() - i_pnt) + length(l2.point2() - i_pnt);
592 
593   return vgl_near_zero(l1_len - l1_idist) && vgl_near_zero(l2_len - l2_idist);
594 }
595 
596 //: Return the intersection point of two lines, if concurrent.
597 // \relatesalso vgl_infinite_line_3d
598 template <class T>
vgl_intersection(vgl_infinite_line_3d<T> const & l1,vgl_infinite_line_3d<T> const & l2,vgl_point_3d<T> & i_pnt)599 bool vgl_intersection(vgl_infinite_line_3d<T> const& l1,
600                       vgl_infinite_line_3d<T> const& l2,
601                       vgl_point_3d<T>& i_pnt)
602 {
603   vgl_line_3d_2_points<T> l21(l1.point(),l1.point_t(T(1)));
604   vgl_line_3d_2_points<T> l22(l2.point(),l2.point_t(T(1)));
605   if (!concurrent(l21, l22))
606     return false;
607   i_pnt = vgl_intersection(l21, l22);
608   return l1.contains(i_pnt) && l2.contains(i_pnt);
609 }
610 
611 template <class T>
vgl_intersection(vgl_ray_3d<T> const & r1,vgl_ray_3d<T> const & r2,vgl_point_3d<T> & i_pnt)612 bool vgl_intersection(vgl_ray_3d<T> const& r1,
613                       vgl_ray_3d<T> const& r2,
614                       vgl_point_3d<T>& i_pnt)
615 {
616   vgl_line_3d_2_points<T> l21(r1.origin(),r1.origin()+r1.direction());
617   vgl_line_3d_2_points<T> l22(r2.origin(),r2.origin()+r2.direction());
618   if (!concurrent(l21, l22))
619     return false;
620   i_pnt = vgl_intersection(l21, l22);
621   return r1.contains(i_pnt)&&r2.contains(i_pnt);
622 }
623 
624 //: Return the intersection point of a line and a plane.
625 // \relatesalso vgl_line_3d_2_points
626 // \relatesalso vgl_plane_3d
627 template <class T>
vgl_intersection(vgl_line_3d_2_points<T> const & line,vgl_plane_3d<T> const & plane)628 vgl_point_3d<T> vgl_intersection(vgl_line_3d_2_points<T> const& line,
629                                  vgl_plane_3d<T> const& plane)
630 {
631   vgl_vector_3d<T> dir = line.direction();
632 
633   vgl_point_3d<T> pt;
634 
635   double denom = plane.a()*dir.x() +
636                  plane.b()*dir.y() +
637                  plane.c()*dir.z();
638 
639   if (denom == 0)
640   {
641     const T inf = std::numeric_limits<T>::infinity();
642     // Line is either parallel or coplanar
643     // If the distance from a line endpoint to the plane is zero, coplanar
644     if (vgl_distance(line.point1(), plane)==0.0)
645       pt.set(inf,inf,inf);
646     else
647       pt.set(inf,0,0);
648   }
649   else
650   {
651     // Infinite line intersects plane
652     double numer = - plane.a()*line.point1().x()
653                    - plane.b()*line.point1().y()
654                    - plane.c()*line.point1().z()
655                    - plane.d();
656 
657     dir *= numer/denom;
658     pt = line.point1() + dir;
659   }
660 
661   return pt;
662 }
663 
664 //: Return the intersection point of a line and a plane.
665 // \relatesalso vgl_line_segment_3d
666 // \relatesalso vgl_plane_3d
667 template <class T>
vgl_intersection(vgl_line_segment_3d<T> const & line,vgl_plane_3d<T> const & plane,vgl_point_3d<T> & i_pt)668 bool vgl_intersection(vgl_line_segment_3d<T> const& line,
669                       vgl_plane_3d<T> const& plane,
670                       vgl_point_3d<T> & i_pt)
671 {
672   vgl_vector_3d<T> dir = line.direction();
673 
674   // The calculation of both denom and numerator are both very dodgy numerically, especially if
675   // denom or numerator is small compared to the summands. It would be good to find a more
676   // numerically stable solution. IMS.
677   const double tol = std::numeric_limits<T>::epsilon() * 10e3;
678 
679   double denom = plane.a()*dir.x() +
680                  plane.b()*dir.y() +
681                  plane.c()*dir.z();
682 
683   if (std::abs(denom) < tol)
684   {
685     // Line is either parallel or coplanar
686     // If the distance from a line endpoint to the plane is zero, coplanar
687     if (vgl_distance(line.point1(), plane)!=0.0)
688       return false;
689 
690     const T inf = std::numeric_limits<T>::infinity();
691     i_pt.set(inf,inf,inf);
692     return true;
693   }
694 
695   double numer = - plane.a()*line.point1().x()
696                  - plane.b()*line.point1().y()
697                  - plane.c()*line.point1().z()
698                  - plane.d();
699 
700   double t = numer/denom; // 0<t<1 between two points
701   if (t < 0 || t > 1.0) return false;
702 
703   i_pt = line.point1() + t * dir;
704   return true;
705 }
706 
707 template <class T>
vgl_intersection(vgl_infinite_line_3d<T> const & line,vgl_plane_3d<T> const & plane,vgl_point_3d<T> & i_pt)708 bool vgl_intersection(vgl_infinite_line_3d<T> const& line,
709                       vgl_plane_3d<T> const& plane,
710                       vgl_point_3d<T> & i_pt)
711 {
712   vgl_vector_3d<T> dir = line.direction();
713   vgl_point_3d<T> pt = line.point();
714   // The calculation of both denom and numerator are both very dodgy numerically, especially if
715   // denom or numerator is small compared to the summands. It would be good to find a more
716   // numerically stable solution. IMS.
717   const double tol = std::numeric_limits<T>::epsilon() * 10e3;
718 
719   double denom = plane.a()*dir.x() +
720                  plane.b()*dir.y() +
721                  plane.c()*dir.z();
722 
723   if (std::abs(denom) < tol)
724   {
725     // Line is either parallel or coplanar
726     // If the distance from a line endpoint to the plane is zero, coplanar
727     if (vgl_distance(pt, plane)!=0.0)
728       return false;
729 
730     const T inf = std::numeric_limits<T>::infinity();
731     i_pt.set(inf,inf,inf);
732     return true;
733   }
734 
735   double numer = - plane.a()*pt.x()
736                  - plane.b()*pt.y()
737                  - plane.c()*pt.z()
738                  - plane.d();
739 
740   double t = numer/denom;
741   i_pt = pt + t * dir;
742   return true;
743 }
744 
745 template <class T>
vgl_intersection(vgl_ray_3d<T> const & ray,vgl_plane_3d<T> const & plane,vgl_point_3d<T> & i_pt)746 bool vgl_intersection(vgl_ray_3d<T> const& ray,
747                       vgl_plane_3d<T> const& plane,
748                       vgl_point_3d<T> & i_pt)
749 {
750   vgl_vector_3d<T> dir = ray.direction();
751   vgl_point_3d<T> pt = ray.origin();
752   // The calculation of both denom and numerator are both very dodgy numerically, especially if
753   // denom or numerator is small compared to the summands. It would be good to find a more
754   // numerically stable solution. IMS.
755   const double tol = std::numeric_limits<T>::epsilon() * 10e3;
756 
757   double denom = plane.a()*dir.x() +
758                  plane.b()*dir.y() +
759                  plane.c()*dir.z();
760 
761   if (std::abs(denom) < tol)
762   {
763     // Line is either parallel or coplanar
764     // If the distance from a line endpoint to the plane is zero, coplanar
765     if (vgl_distance(pt, plane)!=0.0)
766       return false;
767 
768     const T inf = std::numeric_limits<T>::infinity();
769     i_pt.set(inf,inf,inf);
770     return true;
771   }
772 
773   double numer = - plane.a()*pt.x()
774                  - plane.b()*pt.y()
775                  - plane.c()*pt.z()
776                  - plane.d();
777 
778   double t = numer/denom;
779   if (t<0) return false;
780   i_pt = pt + t * dir;
781   return true;
782 }
783 
784 template <class T>
vgl_intersection(const vgl_line_2d<T> & line0,const vgl_line_2d<T> & line1,vgl_point_2d<T> & intersection_point)785 bool vgl_intersection( const vgl_line_2d<T> &line0,
786                        const vgl_line_2d<T> &line1,
787                        vgl_point_2d<T>      &intersection_point )
788 {
789   T a0, b0, c0,  a1, b1, c1;
790   a0 = line0.a(); b0 = line0.b(); c0 = line0.c();
791   a1 = line1.a(); b1 = line1.b(); c1 = line1.c();
792 
793   T delta, delta_x, delta_y, x, y;
794   delta = a0*b1 - a1*b0;
795   if ( std::abs(delta) <= vgl_tolerance<T>::position ) // Lines are parallel
796     return false;
797   delta_x = -c0*b1 + b0*c1; delta_y = -a0*c1 + a1*c0;
798   x = delta_x / delta; y = delta_y / delta;
799 
800   //   intersection_point.set( (Type)x, (Type)y );
801   intersection_point.set( x, y );
802   return true;
803 }
804 
805 //: Return the intersection line of two planes. Returns false if planes
806 // are effectively parallel
807 // \relatesalso vgl_infinite_line_3d
808 // \relatesalso vgl_plane_3d
809 template <class T>
vgl_intersection(vgl_plane_3d<T> const & plane0,vgl_plane_3d<T> const & plane1,vgl_infinite_line_3d<T> & line)810 bool vgl_intersection(vgl_plane_3d<T> const& plane0,
811                       vgl_plane_3d<T> const& plane1,
812                       vgl_infinite_line_3d<T>& line)
813 {
814   double n0x = static_cast<double>(plane0.a());
815   double n0y = static_cast<double>(plane0.b());
816   double n0z = static_cast<double>(plane0.c());
817   double n1x = static_cast<double>(plane1.a());
818   double n1y = static_cast<double>(plane1.b());
819   double n1z = static_cast<double>(plane1.c());
820   vgl_vector_3d<double> n0(n0x, n0y, n0z);
821   vgl_vector_3d<double> n1(n1x, n1y, n1z);
822   // t is the direction vector of the line
823   vgl_vector_3d<double> t = cross_product(n0, n1);
824   double mag = t.length();
825   if (vgl_near_zero(mag)) // planes are parallel (or coincident)
826     return false;
827   t/=mag; // create unit vector
828   double tx = std::fabs(static_cast<double>(t.x_));
829   double ty = std::fabs(static_cast<double>(t.y_));
830   double tz = std::fabs(static_cast<double>(t.z_));
831   // determine maximum component of t
832   char component = 'x';
833   if (ty>=tx&&ty>=tz)
834     component = 'y';
835   if (tz>=tx&&tz>=ty)
836     component = 'z';
837   double d0 = static_cast<double>(plane0.d());
838   double d1 = static_cast<double>(plane1.d());
839   vgl_point_3d<double> p0d;
840   switch (component)
841   {
842     // x is the largest component of t
843     case 'x':
844     {
845       double det = n0y*n1z-n1y*n0z;
846       if (vgl_near_zero(det))
847         return false;
848       double neuy = d1*n0z - d0*n1z;
849       double neuz = d0*n1y - d1*n0y;
850       p0d.set(0.0, neuy/det, neuz/det);
851       break;
852     }
853     case 'y':
854     {
855       double det = n0x*n1z-n1x*n0z;
856       if (vgl_near_zero(det))
857         return false;
858       double neux = d1*n0z - d0*n1z;
859       double neuz = d0*n1x - d1*n0x;
860       p0d.set(neux/det, 0.0, neuz/det);
861       break;
862     }
863     case 'z':
864     default:
865     {
866       double det = n0x*n1y-n1x*n0y;
867       if (vgl_near_zero(det))
868         return false;
869       double neux = d1*n0y - d0*n1y;
870       double neuy = d0*n1x - d1*n0x;
871       p0d.set(neux/det, neuy/det, 0.0);
872       break;
873     }
874   }
875   vgl_point_3d<T> p0(static_cast<T>(p0d.x()),
876                      static_cast<T>(p0d.y()),
877                      static_cast<T>(p0d.z()));
878   vgl_vector_3d<T> tt(static_cast<T>(t.x()),
879                       static_cast<T>(t.y()),
880                       static_cast<T>(t.z()));
881   line = vgl_infinite_line_3d<T>(p0, tt);
882   return true;
883 }
884 
885 //: Return the intersection point of three planes.
886 // \relatesalso vgl_plane_3d
887 template <class T>
vgl_intersection(const vgl_plane_3d<T> & p1,const vgl_plane_3d<T> & p2,const vgl_plane_3d<T> & p3)888 vgl_point_3d<T> vgl_intersection(const vgl_plane_3d<T>& p1,
889                                  const vgl_plane_3d<T>& p2,
890                                  const vgl_plane_3d<T>& p3)
891 {
892   vgl_point_3d<T> p(p1, p2, p3);
893   return p;
894 }
895 
896 //: Return true if any point on [p1,p2] is within tol of [q1,q2]
897 //  Tests two line segments for intersection or near intersection
898 //  (within given tolerance).
899 // \author Dan jackson
900 template <class T>
vgl_intersection(vgl_point_2d<T> const & p1,vgl_point_2d<T> const & p2,vgl_point_2d<T> const & q1,vgl_point_2d<T> const & q2,double tol)901 bool vgl_intersection(vgl_point_2d<T> const& p1,
902                       vgl_point_2d<T> const& p2,
903                       vgl_point_2d<T> const& q1,
904                       vgl_point_2d<T> const& q2,
905                       double tol)
906 {
907   vgl_vector_2d<T> u = p2 - p1;
908   vgl_vector_2d<T> v(-u.y(),u.x());
909   double uq1 = dot_product(q1 - p1,u);
910   double vq1 = dot_product(q1 - p1,v);
911   double tol2 = tol*tol;
912   double L2 = sqr_length(u);
913 
914   // Check if q1 is in central band (either side of line p1-p2
915   if (uq1 > 0 && uq1 < L2)
916   {
917     // Check if q1 is within tol of the line, ie |vq1/L| < tol
918     if (vq1*vq1 <=tol2*L2) return true;
919   }
920   else
921   {
922     // Check if q1 is within tol of either end of line
923     if ( (q1-p1).sqr_length() <= tol2 || (q1-p2).sqr_length() <= tol2 )
924       return true;
925   }
926 
927   // Repeat test for q2
928   double uq2 = dot_product(q2 - p1,u);
929   double vq2 = dot_product(q2 - p1,v);
930 
931   // Check if q2 is in central band (either side of line p1-p2
932   if (uq2 > 0 && uq2 < L2)
933   {
934     // Check if q1 is within tol of the line, ie |vq1/L| < tol
935     if (vq2*vq2 <=tol2*L2)
936       return true;
937   }
938   else
939   {
940     // Check if q1 is within tol of either end of line
941     if ( (q2-p1).sqr_length() <= tol2 || (q2-p2).sqr_length() <= tol2 )
942       return true;
943   }
944 
945   // The points q1 and q2 do not lie within the tolerance region
946   // around line segment (p1,p2)
947   // Now repeat the test the other way around,
948   // testing whether points p1 and p2 lie in tolerance region
949   // of line (q1,q2)
950 
951   u = q2 - q1;
952   v.set(-u.y(),u.x());
953   L2 = sqr_length(u);
954 
955   double up1 = dot_product(p1 - q1,u);
956   double vp1 = dot_product(p1 - q1,v);
957 
958   // Check if p1 is in central band either side of line q1-q2
959   if (up1 > 0 && up1 < L2)
960   {
961     // Check if p1 is within tol of the line, ie |vp1/L| < tol
962     if (vp1*vp1 <=tol2*L2)
963       return true;
964   }
965   else
966   {
967     // Check if p1 is within tol of either end of line
968     if ( (p1-q1).sqr_length() <= tol2 || (p1-q2).sqr_length() <= tol2 )
969       return true;
970   }
971 
972   double up2 = dot_product(p2 - q1,u);
973   double vp2 = dot_product(p2 - q1,v);
974 
975   // Check if p2 is in central band either side of line q1-q2
976   if (up2 > 0 && up2 < L2)
977   {
978     // Check if p1 is within tol of the line, ie |vp1/L| < tol
979     if (vp2*vp2 <=tol2*L2)
980       return true;
981   }
982   else
983   {
984     // Check if p2 is within tol of either end of line
985     if ( (p2-q1).sqr_length() <= tol2 || (p2-q2).sqr_length() <= tol2)
986       return true;
987   }
988 
989   // Now check for actual intersection
990   return vq1*vq2 < 0 && vp1*vp2 < 0;
991 }
992 
993 template <class T>
vgl_intersection(const vgl_box_2d<T> & b,const vgl_polygon<T> & poly)994 bool vgl_intersection(const vgl_box_2d<T>& b,
995                       const vgl_polygon<T>& poly)
996 {
997   // easy checks first
998   // check if any poly vertices are inside the box
999   unsigned int ns = poly.num_sheets();
1000   bool hit = false;
1001   for (unsigned int s = 0; s<ns&&!hit; ++s) {
1002     unsigned int n = (unsigned int)(poly[s].size());
1003     for (unsigned int i = 0; i<n&&!hit; ++i) {
1004       vgl_point_2d<T> p = poly[s][i];
1005       hit = b.contains(p.x(), p.y());
1006     }
1007   }
1008   if (hit) return true;
1009   // check if any box vertices are inside the polygon
1010   T minx = b.min_x(), maxx = b.max_x();
1011   T miny = b.min_y(), maxy = b.max_y();
1012   hit = poly.contains(minx, miny) || poly.contains(maxx, maxy) ||
1013     poly.contains(minx, maxy) || poly.contains(maxx, miny);
1014   if (hit) return true;
1015   // check if any polygon edges intersect the box
1016   for (unsigned int s = 0; s<ns&&!hit; ++s)
1017   {
1018     unsigned int n = (unsigned int)(poly[s].size());
1019     vgl_point_2d<T> ia, ib;
1020     vgl_point_2d<T> last = poly[s][0];
1021     for (unsigned int i = 1; i<n&&!hit; ++i)
1022     {
1023       vgl_point_2d<T> p = poly[s][i];
1024       vgl_line_segment_2d<T> l(last, p);
1025       hit = vgl_intersection<T>(b, l, ia, ib)>0;
1026       last = p;
1027     }
1028     if (!hit) {
1029       vgl_point_2d<T> start = poly[s][0];
1030       vgl_line_segment_2d<T> ll(last, start);
1031       hit = vgl_intersection<T>(b,ll, ia, ib)>0;
1032     }
1033   }
1034   return hit;
1035 }
1036 
1037 //: Return the points from the list that lie inside the box
1038 // \relatesalso vgl_point_2d
1039 // \relatesalso vgl_box_2d
1040 template <class T>
vgl_intersection(vgl_box_2d<T> const & b,std::vector<vgl_point_2d<T>> const & p)1041 std::vector<vgl_point_2d<T> > vgl_intersection(vgl_box_2d<T> const& b, std::vector<vgl_point_2d<T> > const& p)
1042 {
1043   std::vector<vgl_point_2d<T> > r;
1044   typename std::vector<vgl_point_2d<T> >::const_iterator i;
1045   for (i = p.begin(); i != p.end(); ++i)
1046     if (vgl_intersection(b, *i))
1047       r.push_back(*i);
1048   return r;
1049 }
1050 
1051 //: Return the points from the list that lie inside the box
1052 // \relatesalso vgl_point_2d
1053 // \relatesalso vgl_box_2d
1054 template <class T>
vgl_intersection(std::vector<vgl_point_2d<T>> const & p,vgl_box_2d<T> const & b)1055 std::vector<vgl_point_2d<T> > vgl_intersection(std::vector<vgl_point_2d<T> > const& p, vgl_box_2d<T> const& b)
1056 {
1057   std::vector<vgl_point_2d<T> > r;
1058   typename std::vector<vgl_point_2d<T> >::const_iterator i;
1059   for (i = p.begin(); i != p.end(); ++i)
1060     if (vgl_intersection(b, *i))
1061       r.push_back(*i);
1062   return r;
1063 }
1064 
1065 //: Return the points from the list that lie inside the box
1066 // \relatesalso vgl_point_3d
1067 // \relatesalso vgl_box_3d
1068 template <class T>
vgl_intersection(vgl_box_3d<T> const & b,std::vector<vgl_point_3d<T>> const & p)1069 std::vector<vgl_point_3d<T> > vgl_intersection(vgl_box_3d<T> const& b, std::vector<vgl_point_3d<T> > const& p)
1070 {
1071   std::vector<vgl_point_3d<T> > r;
1072   typename std::vector<vgl_point_3d<T> >::const_iterator i;
1073   for (i = p.begin(); i != p.end(); ++i)
1074     if (vgl_intersection(b, *i))
1075       r.push_back(*i);
1076   return r;
1077 }
1078 
1079 //: Return the points from the list that lie inside the box
1080 // \relatesalso vgl_point_3d
1081 // \relatesalso vgl_box_3d
1082 template <class T>
vgl_intersection(std::vector<vgl_point_3d<T>> const & p,vgl_box_3d<T> const & b)1083 std::vector<vgl_point_3d<T> > vgl_intersection(std::vector<vgl_point_3d<T> > const& p, vgl_box_3d<T> const& b)
1084 {
1085   std::vector<vgl_point_3d<T> > r;
1086   typename std::vector<vgl_point_3d<T> >::const_iterator i;
1087   for (i = p.begin(); i != p.end(); ++i)
1088     if (vgl_intersection(b, *i))
1089       r.push_back(*i);
1090   return r;
1091 }
1092 
1093 template <class T>
vgl_intersection(vgl_polygon<T> const & poly,vgl_line_2d<T> const & line)1094 std::vector<vgl_point_2d<T> > vgl_intersection(vgl_polygon<T> const& poly,
1095                                               vgl_line_2d<T> const& line) {
1096   std::vector<vgl_point_2d<T> > ret;
1097   T tol = std::sqrt(vgl_tolerance<T>::position);
1098   T a = line.a(), b = line.b(), c = line.c();
1099   T norm = std::sqrt(a*a + b*b);
1100   a/=norm; b/=norm; c/=norm;
1101   unsigned ns = poly.num_sheets();
1102   for (unsigned s = 0; s<ns; ++s) {
1103     std::vector<vgl_point_2d<T> > sh = poly[s];
1104     unsigned nv = static_cast<unsigned>(sh.size());
1105     for (unsigned i = 0; i<nv; ++i) {
1106       unsigned next = (i+1)%nv;
1107       vgl_point_2d<T> pa = sh[i];
1108       vgl_point_2d<T> pb = sh[next];
1109       //algebraic distances
1110       T ad_a = a*pa.x() +b*pa.y() +c;
1111       T ad_b = a*pb.x() +b*pb.y() +c;
1112       bool sign_a = ad_a>T(0);
1113       bool sign_b = ad_b>T(0);
1114       bool zero = std::abs(ad_a)<tol;
1115       //cases
1116       // 1) no intersections
1117       // 2) current vertex intersects
1118       // 3) intersection interior to poly edge
1119       // case 1
1120       if (!zero&&(sign_a == sign_b))
1121         continue;
1122       // case 2
1123       if (zero) {
1124         ret.push_back(pa);
1125         continue;
1126       }
1127       //case 3
1128       // find the intersection
1129       vgl_line_2d<T> edge(pa, pb);
1130       vgl_point_2d<T> p_int;
1131       if (!vgl_intersection(line, edge, p_int))
1132         continue;
1133       ret.push_back(p_int);
1134     }
1135   }
1136   return ret;
1137 }
1138 
1139 //: find points that intersect the plane within the specified tolerance on normal distance to the plane.
1140 template <class T>
vgl_intersection(vgl_plane_3d<T> const & plane,vgl_pointset_3d<T> const & ptset,T tol)1141 vgl_pointset_3d<T> vgl_intersection(vgl_plane_3d<T> const& plane, vgl_pointset_3d<T> const& ptset, T tol){
1142   vgl_pointset_3d<T> ret;
1143   bool hasn = ptset.has_normals();
1144   unsigned npts = ptset.npts();
1145   for(unsigned i = 0; i<npts; ++i){
1146     vgl_point_3d<T> p = ptset.p(i);
1147     vgl_point_3d<T> cp = vgl_closest_point(plane, p);
1148     T d = static_cast<T>((p-cp).length());
1149     if(d<tol){
1150       if(hasn){
1151         vgl_vector_3d<T> norm = ptset.n(i);
1152         ret.add_point_with_normal(p, norm);
1153       }else{
1154         ret.add_point(p);
1155       }
1156     }
1157   }
1158   return ret;
1159 }
1160 template <class T>
vgl_intersection(vgl_box_3d<T> const & box,vgl_pointset_3d<T> const & ptset)1161 vgl_pointset_3d<T> vgl_intersection(vgl_box_3d<T> const& box, vgl_pointset_3d<T> const& ptset){
1162   unsigned npts = ptset.npts();
1163   std::vector<vgl_point_3d<T> > pts;
1164   std::vector<vgl_vector_3d<T> > normals;
1165   bool hasn = ptset.has_normals();
1166   for(unsigned i = 0; i<npts; ++i){
1167     vgl_point_3d<T> p = ptset.p(i);
1168     if(box.contains(p)){
1169       pts.push_back(p);
1170       if(hasn)
1171         normals.push_back(ptset.n(i));
1172     }
1173   }
1174   if(hasn)
1175     return vgl_pointset_3d<T>(pts, normals);
1176   return vgl_pointset_3d<T>(pts);
1177 }
1178 
1179 // Instantiate those functions which are suitable for integer instantiation.
1180 #undef VGL_INTERSECTION_BOX_INSTANTIATE
1181 #define VGL_INTERSECTION_BOX_INSTANTIATE(T) \
1182 template vgl_box_2d<T > vgl_intersection(vgl_box_2d<T > const&,vgl_box_2d<T > const&); \
1183 template vgl_box_3d<T > vgl_intersection(vgl_box_3d<T > const&,vgl_box_3d<T > const&); \
1184 template std::vector<vgl_point_2d<T > > vgl_intersection(vgl_box_2d<T > const&,std::vector<vgl_point_2d<T > > const&); \
1185 template std::vector<vgl_point_2d<T > > vgl_intersection(std::vector<vgl_point_2d<T > > const&,vgl_box_2d<T > const&); \
1186 template std::vector<vgl_point_3d<T > > vgl_intersection(vgl_box_3d<T > const&,std::vector<vgl_point_3d<T > > const&); \
1187 template std::vector<vgl_point_3d<T > > vgl_intersection(std::vector<vgl_point_3d<T > > const&,vgl_box_3d<T > const&); \
1188 template bool vgl_intersection(vgl_box_2d<T > const&,vgl_line_2d<T > const&,vgl_point_2d<T >&,vgl_point_2d<T >&); \
1189 template bool vgl_intersection(vgl_box_2d<T> const&,vgl_line_segment_2d<T> const&, vgl_line_segment_2d<T>& ); \
1190 template bool vgl_intersection(vgl_box_3d<T > const&,vgl_plane_3d<T > const&); \
1191 template bool vgl_intersection(vgl_box_3d<T > const&,vgl_infinite_line_3d<T > const&,vgl_point_3d<T >&,vgl_point_3d<T >&);\
1192 template bool vgl_intersection(vgl_box_3d<T > const&,vgl_ray_3d<T > const&,vgl_point_3d<T >&,vgl_point_3d<T >&)
1193 #undef VGL_INTERSECTION_INSTANTIATE
1194 #define VGL_INTERSECTION_INSTANTIATE(T) \
1195 template vgl_pointset_3d<T> vgl_intersection(vgl_plane_3d<T> const&, vgl_pointset_3d<T> const&, T); \
1196 template vgl_pointset_3d<T> vgl_intersection(vgl_box_3d<T> const&, vgl_pointset_3d<T> const&); \
1197 template vgl_point_3d<T > vgl_intersection(vgl_line_3d_2_points<T > const&,vgl_line_3d_2_points<T > const&); \
1198 template bool vgl_intersection(vgl_line_segment_2d<T> const&, vgl_line_segment_2d<T> const&, vgl_point_2d<T>&); \
1199 template bool vgl_intersection(vgl_line_segment_3d<T > const&,vgl_line_segment_3d<T > const&,vgl_point_3d<T >&); \
1200 template bool vgl_intersection(vgl_line_3d_2_points<T > const&,vgl_line_segment_3d<T > const&,vgl_point_3d<T >&); \
1201 template bool vgl_intersection(vgl_ray_3d<T > const&,vgl_ray_3d<T > const&,vgl_point_3d<T >&); \
1202 template vgl_point_3d<T > vgl_intersection(vgl_line_3d_2_points<T > const&,vgl_plane_3d<T > const&); \
1203 template bool vgl_intersection(vgl_line_segment_3d<T > const&,vgl_plane_3d<T > const&,vgl_point_3d<T >&); \
1204 template bool vgl_intersection(vgl_infinite_line_3d<T > const&,vgl_plane_3d<T > const&,vgl_point_3d<T >&); \
1205 template bool vgl_intersection(vgl_ray_3d<T > const& ray, vgl_plane_3d<T > const& plane, vgl_point_3d<T > & i_pt); \
1206 template bool vgl_intersection(vgl_infinite_line_3d<T > const&,vgl_infinite_line_3d<T > const&,vgl_point_3d<T >&); \
1207 template vgl_point_3d<T > vgl_intersection(vgl_plane_3d<T > const&,vgl_plane_3d<T > const&,vgl_plane_3d<T > const&); \
1208 template unsigned vgl_intersection(vgl_box_2d<T > const&,vgl_line_segment_2d<T > const&,vgl_point_2d<T >&,vgl_point_2d<T >&); \
1209 template bool vgl_intersection(vgl_line_2d<T > const&,vgl_line_2d<T > const&,vgl_point_2d<T >&); \
1210 template bool vgl_intersection(vgl_point_2d<T > const&,vgl_point_2d<T > const&,vgl_point_2d<T > const&,vgl_point_2d<T > const&,double); \
1211 template bool vgl_intersection(vgl_box_2d<T > const&,vgl_polygon<T > const&); \
1212 template bool vgl_intersection(vgl_plane_3d<T > const&,vgl_plane_3d<T > const&,vgl_line_segment_3d<T > &); \
1213 template bool vgl_intersection(vgl_plane_3d<T > const&,vgl_plane_3d<T > const&,vgl_infinite_line_3d<T >&); \
1214 template std::vector<vgl_point_2d<T > > vgl_intersection(vgl_polygon<T > const&, vgl_line_2d<T > const&); \
1215 VGL_INTERSECTION_BOX_INSTANTIATE(T)
1216 
1217 #endif // vgl_intersection_hxx_
1218