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