wfmath  1.0.3
A math library for the Worldforge system.
polygon_intersect.cpp
1 // polygon_intersect.cpp (Polygon<2> intersection functions)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2002 Ron Steinke and The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 
23 // Author: Ron Steinke
24 // Created: 2002-2-20
25 
26 #include "polygon_intersect.h"
27 
28 #include "segment.h"
29 #include "rotbox.h"
30 
31 #include <algorithm>
32 #include <list>
33 #include <cassert>
34 
35 namespace WFMath {
36 
37 
38 
39 template<int dim>
40 inline Vector<dim> Poly2Orient<dim>::offset(const Point<dim>& pd, Point<2>& p2) const
41 {
42  assert(m_origin.isValid()); // Check for empty polygon before calling this
43 
44  Vector<dim> out = pd - m_origin;
45 
46  for(int j = 0; j < 2; ++j) {
47  p2[j] = Dot(out, m_axes[j]);
48  out -= p2[j] * m_axes[j];
49  }
50 
51  return out;
52 }
53 
54 template<int dim>
55 inline bool Poly2Orient<dim>::checkContained(const Point<dim>& pd, Point<2> & p2) const
56 {
57  Vector<dim> off = offset(pd, p2);
58 
59  CoordType sqrsum = 0;
60  for(int i = 0; i < dim; ++i)
61  sqrsum += pd[i] * pd[i];
62 
63  return off.sqrMag() < numeric_constants<CoordType>::epsilon() * sqrsum;
64 }
65 
66 template<>
67 bool Poly2Orient<3>::checkIntersectPlane(const AxisBox<3>& b, Point<2>& p2,
68  bool proper) const;
69 
70 template<int dim>
71 bool Poly2Orient<dim>::checkIntersect(const AxisBox<dim>& b, Point<2>& p2,
72  bool proper) const
73 {
74  assert(m_origin.isValid());
75 
76  if(!m_axes[0].isValid()) {
77  // Single point
78  p2[0] = p2[1] = 0;
79  return Intersect(b, convert(p2), proper);
80  }
81 
82  if(m_axes[1].isValid()) {
83  // A plane
84 
85  // I only know how to do this in 3D, so write a function which will
86  // specialize to different dimensions
87 
88  return checkIntersectPlane(b, p2, proper);
89  }
90 
91  // A line
92 
93  // This is a modified version of AxisBox<>/Segment<> intersection
94 
95  CoordType min = 0, max = 0; // Initialize to avoid compiler warnings
96  bool got_bounds = false;
97 
98  for(int i = 0; i < dim; ++i) {
99  const CoordType dist = (m_axes[0])[i]; // const may optimize away better
100  if(dist == 0) {
101  if(_Less(m_origin[i], b.lowCorner()[i], proper)
102  || _Greater(m_origin[i], b.highCorner()[i], proper))
103  return false;
104  }
105  else {
106  CoordType low = (b.lowCorner()[i] - m_origin[i]) / dist;
107  CoordType high = (b.highCorner()[i] - m_origin[i]) / dist;
108  if(low > high) {
109  CoordType tmp = high;
110  high = low;
111  low = tmp;
112  }
113  if(got_bounds) {
114  if(low > min)
115  min = low;
116  if(high < max)
117  max = high;
118  }
119  else {
120  min = low;
121  max = high;
122  got_bounds = true;
123  }
124  }
125  }
126 
127  assert(got_bounds); // We can't be parallel in _all_ dimensions
128 
129  if(_LessEq(min, max, proper)) {
130  p2[0] = (max - min) / 2;
131  p2[1] = 0;
132  return true;
133  }
134  else
135  return false;
136 }
137 
138 template<int dim>
139 int Intersect(const Poly2Orient<dim> &o1, const Poly2Orient<dim> &o2,
140  Poly2OrientIntersectData &data)
141 {
142  if(!o1.m_origin.isValid() || !o2.m_origin.isValid()) { // No points
143  return -1;
144  }
145 
146  // Check for single point basis
147 
148  if(!o1.m_axes[0].isValid()) {
149  if(!o2.checkContained(o1.m_origin, data.p2))
150  return -1; // no intersect
151 
152  //Poly2OrientIntersectData data;
153 
154  data.p1[0] = data.p1[1] = 0;
155 
156  return 0; // point intersect
157  }
158 
159  if(!o2.m_axes[0].isValid()) {
160  if(!o1.checkContained(o2.m_origin, data.p1))
161  return -1; // no intersect
162 
163  data.p2[0] = data.p2[1] = 0;
164 
165  return 0; // point intersect
166  }
167 
168  // Find a common basis for the plane's orientations
169  // by projecting out the part of o1's basis that lies
170  // in o2's basis
171 
172  Vector<dim> basis1, basis2;
173  CoordType sqrmag1, sqrmag2;
174  int basis_size = 0;
175 
176  basis1 = o2.m_axes[0] * Dot(o2.m_axes[0], o1.m_axes[0]);
177  if(o2.m_axes[1].isValid())
178  basis1 += o2.m_axes[1] * Dot(o2.m_axes[1], o1.m_axes[0]);
179 
180  // Don't need to scale, the m_axes are unit vectors
181  sqrmag1 = basis1.sqrMag();
183  basis_size = 1;
184 
185  if(o1.m_axes[1].isValid()) {
186  basis2 = o2.m_axes[0] * Dot(o2.m_axes[0], o1.m_axes[1]);
187  if(o2.m_axes[1].isValid())
188  basis2 += o2.m_axes[1] * Dot(o2.m_axes[1], o1.m_axes[1]);
189 
190  // Project out part parallel to basis1
191  if(basis_size == 1)
192  basis2 -= basis1 * (Dot(basis1, basis2) / sqrmag1);
193 
194  sqrmag2 = basis2.sqrMag();
196  if(basis_size++ == 0) {
197  basis1 = basis2;
198  sqrmag1 = sqrmag2;
199  }
200  }
201  }
202 
203  Vector<dim> off = o2.m_origin - o1.m_origin;
204 
205  switch(basis_size) {
206  case 0:
207  {
208  // All vectors are orthogonal, check for a common point in the plane
209  // This can happen even in 3d for degenerate bases
210 
211  data.p1[0] = Dot(o1.m_axes[0], off);
212  Vector<dim> off1 = o1.m_axes[0] * data.p1[0];
213  if(o1.m_axes[1].isValid()) {
214  data.p1[1] = Dot(o1.m_axes[1], off);
215  off1 += o1.m_axes[1] * data.p1[1];
216  }
217  else
218  data.p1[1] = 0;
219 
220  data.p2[0] = -Dot(o2.m_axes[0], off);
221  Vector<dim> off2 = o2.m_axes[0] * data.p2[0];
222  if(o1.m_axes[1].isValid()) {
223  data.p2[1] = -Dot(o2.m_axes[1], off);
224  off2 += o1.m_axes[1] * data.p2[1];
225  }
226  else
227  data.p2[1] = 0;
228 
229  if(off1 - off2 != off) // No common point
230  return -1;
231  else // Got a point
232  return 1;
233  }
234  case 1:
235  {
236  // Check for an intersection line
237 
238  data.o1_is_line = !o1.m_axes[1].isValid();
239  data.o2_is_line = !o2.m_axes[1].isValid();
240 
241  if(!o1.m_axes[1].isValid() && !o2.m_axes[1].isValid()) {
242  CoordType proj = Dot(off, o2.m_axes[0]);
243  if(off != o2.m_axes[0] * proj)
244  return -1;
245 
246  data.v1[0] = 1;
247  data.v1[1] = 0;
248  data.p1[0] = data.p1[1] = 0;
249  data.v2[0] = (Dot(o1.m_axes[0], o2.m_axes[0]) > 0) ? 1 : -1;
250  data.v2[1] = 0;
251  data.p2[0] = -proj;
252  data.p2[1] = 0;
253 
254  return 1;
255  }
256 
257  if(!o1.m_axes[1].isValid()) {
258  data.p2[0] = -Dot(off, o2.m_axes[0]);
259  data.p2[1] = -Dot(off, o2.m_axes[1]);
260 
261  if(off != - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
262  return -1;
263 
264  data.v1[0] = 1;
265  data.v1[1] = 0;
266  data.p1[0] = data.p1[1] = 0;
267  data.v2[0] = Dot(o1.m_axes[0], o2.m_axes[0]);
268  data.v2[1] = Dot(o1.m_axes[0], o2.m_axes[1]);
269 
270  return 1;
271  }
272 
273  if(!o2.m_axes[1].isValid()) {
274  data.p1[0] = Dot(off, o1.m_axes[0]);
275  data.p1[1] = Dot(off, o1.m_axes[1]);
276 
277  if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1])
278  return -1;
279 
280  data.v2[0] = 1;
281  data.v2[1] = 0;
282  data.p2[0] = data.p2[1] = 0;
283  data.v1[0] = Dot(o1.m_axes[0], o2.m_axes[0]);
284  data.v1[1] = Dot(o1.m_axes[1], o2.m_axes[0]);
285 
286  return 1;
287  }
288 
289  data.p1[0] = Dot(off, o1.m_axes[0]);
290  data.p1[1] = Dot(off, o1.m_axes[1]);
291  data.p2[0] = -Dot(off, o2.m_axes[0]);
292  data.p2[1] = -Dot(off, o2.m_axes[1]);
293 
294  if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1]
295  - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
296  return -1;
297 
298  basis1 /= std::sqrt(sqrmag1);
299 
300  data.v1[0] = Dot(o1.m_axes[0], basis1);
301  data.v1[1] = Dot(o1.m_axes[1], basis1);
302  data.v2[0] = Dot(o2.m_axes[0], basis1);
303  data.v2[1] = Dot(o2.m_axes[1], basis1);
304 
305  return 1;
306  }
307  case 2:
308  {
309  assert(o1.m_axes[1].isValid() && o2.m_axes[1].isValid());
310 
311  // The planes are parallel, check if they are the same plane
312  CoordType off_sqr_mag = data.off.sqrMag();
313 
314  // Find the offset between the origins in o2's coordnates
315 
316  if(off_sqr_mag != 0) { // The offsets aren't identical
317  Vector<dim> off_copy = off;
318 
319  data.off[0] = Dot(o2.m_axes[0], off);
320  off_copy -= o1.m_axes[0] * data.off[0];
321  data.off[1] = Dot(o2.m_axes[1], off);
322  off_copy -= o1.m_axes[1] * data.off[1];
323 
324  if(off_copy.sqrMag() > off_sqr_mag * numeric_constants<CoordType>::epsilon())
325  return -1; // The planes are different
326  }
327  else
328  data.off[0] = data.off[1] = 0;
329 
330  // Define o2's basis vectors in o1's coordinates
331  data.v1[0] = Dot(o2.m_axes[0], o1.m_axes[0]);
332  data.v1[1] = Dot(o2.m_axes[0], o1.m_axes[1]);
333  data.v2[0] = Dot(o2.m_axes[1], o1.m_axes[0]);
334  data.v2[1] = Dot(o2.m_axes[1], o1.m_axes[1]);
335 
336  return 2;
337  }
338  default:
339  assert(false);
340  return -1;
341  }
342 }
343 
344 template<int dim>
345 inline bool Intersect(const Polygon<dim>& r, const Point<dim>& p, bool proper)
346 {
347  Point<2> p2;
348 
349  return r.m_poly.numCorners() > 0 && r.m_orient.checkContained(p, p2)
350  && Intersect(r.m_poly, p2, proper);
351 }
352 
353 template<int dim>
354 inline bool Contains(const Point<dim>& p, const Polygon<dim>& r, bool proper)
355 {
356  if(r.m_poly.numCorners() == 0)
357  return true;
358 
359  if(proper)
360  return false;
361 
362  for(size_t i = 1; i < r.m_poly.numCorners(); ++i)
363  if(r.m_poly[i] != r.m_poly[0])
364  return false;
365 
366  Point<2> p2;
367 
368  return r.m_orient.checkContained(p, p2) && p2 == r.m_poly[0];
369 }
370 
371 template<int dim>
372 bool Intersect(const Polygon<dim>& p, const AxisBox<dim>& b, bool proper)
373 {
374  size_t corners = p.m_poly.numCorners();
375 
376  if(corners == 0)
377  return false;
378 
379  Point<2> p2;
380 
381  if(!p.m_orient.checkIntersect(b, p2, proper))
382  return false;
383 
384  Segment<dim> s;
385  s.endpoint(0) = p.m_orient.convert(p.m_poly.getCorner(corners-1));
386  int next_end = 1;
387 
388  for(size_t i = 0; i < corners; ++i) {
389  s.endpoint(next_end) = p.m_orient.convert(p.m_poly.getCorner(i));
390  if(Intersect(b, s, proper))
391  return true;
392  next_end = next_end ? 0 : 1;
393  }
394 
395  return Contains(p, p2, proper);
396 }
397 
398 template<int dim>
399 bool _PolyContainsBox(const Poly2Orient<dim> &orient, const Polygon<2> &poly,
400  const Point<dim> &corner, const Vector<dim> &size, bool proper)
401 {
402  int num_dim = 0, nonzero_dim = -1;
403 
404  for(int i = 0; i < dim; ++i) {
405  if(size[i] == 0)
406  continue;
407  if(num_dim == 2)
408  return false;
409  if(nonzero_dim == -1 || std::fabs(size[nonzero_dim]) < std::fabs(size[i]))
410  nonzero_dim = i;
411  ++num_dim;
412  }
413 
414  Point<2> corner1;
415 
416  if(!orient.checkContained(corner, corner1))
417  return false;
418 
419  if(num_dim == 0)
420  return Contains(poly, corner1, proper);
421 
422  Point<2> corner2;
423 
424  if(!orient.checkContained(corner + size, corner2))
425  return false;
426 
427  if(num_dim == 1)
428  return Contains(poly, Segment<2>(corner1, corner2), proper);
429 
430  Point<dim> other_corner = corner;
431  other_corner[nonzero_dim] += size[nonzero_dim];
432 
433  Point<2> corner3;
434  if(!orient.checkContained(other_corner, corner3))
435  return false;
436 
437  // Create a RotBox<2>
438 
439  Vector<2> vec1(corner2 - corner1), vec2(corner3 - corner1);
440 
441  RotMatrix<2> m; // A matrix which gives the rotation from the x-axis to vec1
442 
443  try {
444  m.rotation(Vector<2>(1, 0), vec1);
445  }
446  catch(const ColinearVectors<2>&) { // vec1 is parallel to (-1, 0), so we're fine
447  m.identity();
448  }
449 
450  RotBox<2> box(corner1, ProdInv(vec2, m), m);
451 
452  return Contains(poly, box, proper);
453 }
454 
455 template<int dim>
456 inline bool Contains(const Polygon<dim>& p, const AxisBox<dim>& b, bool proper)
457 {
458  return _PolyContainsBox(p.m_orient, p.m_poly, b.m_low, b.m_high - b.m_low, proper);
459 }
460 
461 template<int dim>
462 inline bool Contains(const AxisBox<dim>& b, const Polygon<dim>& p, bool proper)
463 {
464  for(size_t i = 0; i < p.m_poly.numCorners(); ++i)
465  if(!Contains(b, p.getCorner(i), proper))
466  return false;
467 
468  return true;
469 }
470 
471 template<int dim>
472 inline bool Intersect(const Polygon<dim>& p, const Ball<dim>& b, bool proper)
473 {
474  if(p.m_poly.numCorners() == 0)
475  return false;
476 
477  Point<2> c2;
478  CoordType dist;
479 
480  dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
481 
482  if(_Less(dist, 0, proper))
483  return false;
484 
485  return Intersect(p.m_poly, Ball<2>(c2, std::sqrt(dist)), proper);
486 }
487 
488 template<int dim>
489 inline bool Contains(const Polygon<dim>& p, const Ball<dim>& b, bool proper)
490 {
491  if(p.m_poly.numCorners() == 0)
492  return false;
493 
494  if(b.m_radius > 0)
495  return false;
496 
497  Point<2> c2;
498 
499  if(!p.m_orient.checkContained(b.m_center, c2))
500  return false;
501 
502  return Contains(p.m_poly, c2, proper);
503 }
504 
505 template<int dim>
506 inline bool Contains(const Ball<dim>& b, const Polygon<dim>& p, bool proper)
507 {
508  if(p.m_poly.numCorners() == 0)
509  return true;
510 
511  Point<2> c2;
512  CoordType dist;
513 
514  dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
515 
516  if(_Less(dist, 0, proper))
517  return false;
518 
519  for(size_t i = 0; i != p.m_poly.numCorners(); ++i)
520  if(_Less(dist, SquaredDistance(c2, p.m_poly[i]), proper))
521  return false;
522 
523  return true;
524 }
525 
526 template<int dim>
527 bool Intersect(const Polygon<dim>& p, const Segment<dim>& s, bool proper)
528 {
529  if(p.m_poly.numCorners() == 0)
530  return false;
531 
532  Point<2> p1, p2;
533  CoordType d1, d2;
534  Vector<dim> v1, v2;
535 
536  v1 = p.m_orient.offset(s.m_p1, p1);
537  v2 = p.m_orient.offset(s.m_p2, p2);
538 
539  if(Dot(v1, v2) > 0) // Both points on same side of sheet
540  return false;
541 
542  d1 = v1.mag();
543  d2 = v2.mag();
544  Point<2> p_intersect;
545 
546  if(d1 + d2 == 0) // Avoid divide by zero later
547  return Intersect(p.m_poly, Segment<2>(p1, p2), proper);
548 
549  for(int i = 0; i < 2; ++i)
550  p_intersect[i] = (p1[i] * d2 + p2[i] * d1) / (d1 + d2);
551 
552  return Intersect(p.m_poly, p_intersect, proper);
553 }
554 
555 template<int dim>
556 inline bool Contains(const Polygon<dim>& p, const Segment<dim>& s, bool proper)
557 {
558  if(p.m_poly.numCorners() == 0)
559  return false;
560 
561  Segment<2> s2;
562 
563  if(!p.m_orient.checkContained(s.m_p1, s2.endpoint(0)))
564  return false;
565  if(!p.m_orient.checkContained(s.m_p2, s2.endpoint(1)))
566  return false;
567 
568  return Contains(p.m_poly, s2, proper);
569 }
570 
571 template<int dim>
572 inline bool Contains(const Segment<dim>& s, const Polygon<dim>& p, bool proper)
573 {
574  if(p.m_poly.numCorners() == 0)
575  return true;
576 
577  // Expand the basis to include the segment, this deals well with
578  // degenerate polygons
579 
580  Segment<2> s2;
581  Poly2Orient<dim> orient(p.m_orient);
582 
583  for(int i = 0; i < 2; ++i)
584  if(!orient.expand(s.endpoint(i), s2.endpoint(i)))
585  return false;
586 
587  return Contains(s2, p.m_poly, proper);
588 }
589 
590 template<int dim>
591 bool Intersect(const Polygon<dim>& p, const RotBox<dim>& r, bool proper)
592 {
593  size_t corners = p.m_poly.numCorners();
594 
595  if(corners == 0)
596  return false;
597 
598  Poly2Orient<dim> orient(p.m_orient);
599  // FIXME rotateInverse()
600  orient.rotate(r.m_orient.inverse(), r.m_corner0);
601 
602  AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
603 
604  Point<2> p2;
605 
606  if(!orient.checkIntersect(b, p2, proper))
607  return false;
608 
609  Segment<dim> s;
610  s.endpoint(0) = orient.convert(p.m_poly.getCorner(corners-1));
611  int next_end = 1;
612 
613  for(size_t i = 0; i < corners; ++i) {
614  s.endpoint(next_end) = orient.convert(p.m_poly.getCorner(i));
615  if(Intersect(b, s, proper))
616  return true;
617  next_end = next_end ? 0 : 1;
618  }
619 
620  return Contains(p, p2, proper);
621 }
622 
623 template<int dim>
624 inline bool Contains(const Polygon<dim>& p, const RotBox<dim>& r, bool proper)
625 {
626  Poly2Orient<dim> orient(p.m_orient);
627  orient.rotate(r.m_orient.inverse(), r.m_corner0);
628 
629  return _PolyContainsBox(orient, p.m_poly, r.m_corner0, r.m_size, proper);
630 }
631 
632 template<int dim>
633 inline bool Contains(const RotBox<dim>& r, const Polygon<dim>& p, bool proper)
634 {
635  if(p.m_poly.numCorners() == 0)
636  return true;
637 
638  AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
639 
640  Poly2Orient<dim> orient(p.m_orient);
641  orient.rotate(r.m_orient.inverse(), r.m_corner0);
642 
643  for(size_t i = 0; i < p.m_poly.numCorners(); ++i)
644  if(!Contains(b, orient.convert(p.m_poly[i]), proper))
645  return false;
646 
647  return true;
648 }
649 
650 bool PolyPolyIntersect(const Polygon<2> &poly1, const Polygon<2> &poly2,
651  int intersect_dim,
652  const Poly2OrientIntersectData &data, bool proper);
653 
654 template<int dim>
655 inline bool Intersect(const Polygon<dim>& p1, const Polygon<dim>& p2, bool proper)
656 {
657  Poly2OrientIntersectData data;
658 
659  int intersect_dim = Intersect(p1.m_orient, p2.m_orient, data);
660 
661  return PolyPolyIntersect(p1.m_poly, p2.m_poly, intersect_dim, data, proper);
662 }
663 
664 bool PolyPolyContains(const Polygon<2> &outer, const Polygon<2> &inner,
665  int intersect_dim,
666  const Poly2OrientIntersectData &data, bool proper);
667 
668 template<int dim>
669 inline bool Contains(const Polygon<dim>& outer, const Polygon<dim>& inner, bool proper)
670 {
671  if(outer.m_poly.numCorners() == 0)
672  return !proper && inner.m_poly.numCorners() == 0;
673 
674  if(inner.m_poly.numCorners() == 0)
675  return true;
676 
677  Poly2OrientIntersectData data;
678 
679  int intersect_dim = Intersect(outer.m_orient, inner.m_orient, data);
680 
681  return PolyPolyContains(outer.m_poly, inner.m_poly, intersect_dim, data, proper);
682 }
683 
684 // instantiations, only need 3d because 2d is a specialization,
685 // except for the reverse-order intersect
686 
687 template bool Intersect<Point<2>,Polygon<2> >(const Point<2>&, const Polygon<2>&, bool);
688 template bool Intersect<Point<3>,Polygon<3> >(const Point<3>&, const Polygon<3>&, bool);
689 template bool Contains<3>(const Point<3>&, const Polygon<3>&, bool);
690 template bool Intersect<3>(const Polygon<3>&, const Point<3>&, bool);
691 template bool Contains<3>(const Polygon<3>&, const Point<3>&, bool);
692 
693 template bool Intersect<AxisBox<2>,Polygon<2> >(const AxisBox<2>&, const Polygon<2>&, bool);
694 template bool Intersect<AxisBox<3>,Polygon<3> >(const AxisBox<3>&, const Polygon<3>&, bool);
695 template bool Contains<3>(const AxisBox<3>&, const Polygon<3>&, bool);
696 template bool Intersect<3>(const Polygon<3>&, const AxisBox<3>&, bool);
697 template bool Contains<3>(const Polygon<3>&, const AxisBox<3>&, bool);
698 
699 template bool Intersect<Ball<2>,Polygon<2> >(const Ball<2>&, const Polygon<2>&, bool);
700 template bool Intersect<Ball<3>,Polygon<3> >(const Ball<3>&, const Polygon<3>&, bool);
701 template bool Contains<3>(const Ball<3>&, const Polygon<3>&, bool);
702 template bool Intersect<3>(const Polygon<3>&, const Ball<3>&, bool);
703 template bool Contains<3>(const Polygon<3>&, const Ball<3>&, bool);
704 
705 template bool Intersect<Segment<2>,Polygon<2> >(const Segment<2>&, const Polygon<2>&, bool);
706 template bool Intersect<Segment<3>,Polygon<3> >(const Segment<3>&, const Polygon<3>&, bool);
707 template bool Contains<3>(const Segment<3>&, const Polygon<3>&, bool);
708 template bool Intersect<3>(const Polygon<3>&, const Segment<3>&, bool);
709 template bool Contains<3>(const Polygon<3>&, const Segment<3>&, bool);
710 
711 template bool Intersect<RotBox<2>,Polygon<2> >(const RotBox<2>&, const Polygon<2>&, bool);
712 template bool Intersect<RotBox<3>,Polygon<3> >(const RotBox<3>&, const Polygon<3>&, bool);
713 template bool Contains<3>(const RotBox<3>&, const Polygon<3>&, bool);
714 template bool Intersect<3>(const Polygon<3>&, const RotBox<3>&, bool);
715 template bool Contains<3>(const Polygon<3>&, const RotBox<3>&, bool);
716 
717 template bool Intersect<3>(const Polygon<3>&, const Polygon<3>&, bool);
718 template bool Contains<3>(const Polygon<3>&, const Polygon<3>&, bool);
719 
720 template<>
721 bool Poly2Orient<3>::checkIntersectPlane(const AxisBox<3>& b, Point<2>& p2,
722  bool proper) const
723 {
724  assert("This function should only be called if the orientation represents a plane" &&
725  m_origin.isValid() && m_axes[0].isValid() && m_axes[1].isValid());
726 
727  Vector<3> normal = Cross(m_axes[0], m_axes[1]); // normal to the plane
728 
729 // enum {
730 // AXIS_UP,
731 // AXIS_DOWN,
732 // AXIS_FLAT
733 // } axis_direction[3];
734 
735  CoordType normal_mag = normal.sloppyMag();
736  int high_corner_num = 0;
737 
738  for(int i = 0; i < 3; ++i) {
739  if(std::fabs(normal[i]) < normal_mag * numeric_constants<CoordType>::epsilon()) {
740 // axis_direction[i] = AXIS_FLAT;
741  } else if(normal[i] > 0) {
742 // axis_direction[i] = AXIS_UP;
743  high_corner_num |= (1 << i);
744  }
745 // else
746 // axis_direction[i] = AXIS_DOWN;
747  }
748 
749  int low_corner_num = high_corner_num ^ 7;
750 
751  Point<3> high_corner = b.getCorner(high_corner_num);
752  Point<3> low_corner = b.getCorner(low_corner_num);
753 
754  // If these are on opposite sides of the plane, we have an intersection
755 
756  CoordType perp_size = Dot(normal, high_corner - low_corner) / normal_mag;
757  assert(perp_size >= 0);
758 
759  if(perp_size < normal_mag * numeric_constants<CoordType>::epsilon()) {
760  // We have a very flat box, lying parallel to the plane
761  return !proper && checkContained(Midpoint(high_corner, low_corner), p2);
762  }
763 
764  if(_Less(Dot(high_corner - m_origin, normal), 0, proper)
765  || _Less(Dot(low_corner - m_origin, normal), 0, proper))
766  return false; // box lies above or below the plane
767 
768  // Find the intersection of the line through the corners with the plane
769 
770  Point<2> p2_high, p2_low;
771 
772  CoordType high_dist = offset(high_corner, p2_high).mag();
773  CoordType low_dist = offset(low_corner, p2_low).mag();
774 
775  p2 = Midpoint(p2_high, p2_low, high_dist / (high_dist + low_dist));
776 
777  return true;
778 }
779 
780 // This assumes the y coordinates of the points are all zero
781 static void LinePolyGetBounds(const Polygon<2> &poly, CoordType &low, CoordType &high)
782 {
783  low = high = poly[0][0];
784 
785  for(size_t i = 0; i < poly.numCorners(); ++i) {
786  CoordType val = poly[i][0];
787  if(val < low)
788  low = val;
789  if(val > high)
790  high = val;
791  }
792 }
793 
794 // For use in GetCrossings()
796  CoordType low, high;
797  bool cross;
798 };
799 
800 // This finds the intervals where the polygon intersects the line
801 // through p parallel to v, and puts the endpoints of those
802 // intervals in the vector "cross"
803 static bool GetCrossings(const Polygon<2> &poly, const Point<2> &p,
804  const Vector<2> &v, std::vector<CoordType> &cross,
805  bool proper)
806 {
807  assert(poly.numCorners() == cross.size()); // Already allocated
808  assert(Equal(v.sqrMag(), 1));
809 
810  // The sign of the cross product changes when you cross the line
811  Point<2> old_p = poly.getCorner(poly.numCorners() - 1);
812  bool old_below = (Cross(v, old_p - p) < 0);
813  int next_cross = 0;
814 
815  // Stuff for when multiple sequential corners lie on the line
816  std::list<LinePointData> line_point_data;
817 
818  for(size_t i = 0; i < poly.numCorners(); ++i) {
819  Point<2> p_i = poly.getCorner(i);
820  Vector<2> v_i = p_i - p;
821 
822  CoordType v_i_sqr_mag = v_i.sqrMag(), proj = Dot(v_i, v);
823 
824  if(Equal(v_i_sqr_mag, proj * proj)) { // corner lies on line
825  Point<2> p_j;
826  Vector<2> v_j;
827  CoordType proj_j, low_proj = proj, high_proj = proj;
828  size_t j;
829  for(j = i + 1; j != i; j == poly.numCorners() - 1 ? j = 0 : ++j) {
830  p_j = poly.getCorner(j);
831  v_j = p_j - p;
832  proj_j = Dot(v_j, v);
833 
834  if(!Equal(v_j.sqrMag(), proj_j * proj_j))
835  break;
836 
837  if(proj_j < low_proj)
838  low_proj = proj_j;
839  if(proj_j > high_proj)
840  high_proj = proj_j;
841  }
842 
843  assert(j != i); // We know that the polygon spans a 2d space
844 
845  bool below = (Cross(v, v_j) < 0);
846 
847  if(below == old_below && proper) {
848  old_p = p_j;
849  continue;
850  }
851 
852  if(j == i + 1) { // just one point on the line
853 
854  if(below != old_below) {
855  old_below = below;
856  cross[next_cross++] = proj;
857  }
858  else {
859  assert(!proper);
860  // Just touches, adding it twice will give a zero length "hit" region
861  cross[next_cross++] = proj;
862  cross[next_cross++] = proj;
863  }
864 
865  old_p = p_j;
866  continue;
867  }
868 
869  LinePointData data = {low_proj, high_proj, below != old_below};
870 
871  std::list<LinePointData>::iterator I;
872 
873  for(I = line_point_data.begin(); I != line_point_data.end(); ++I) {
874  if(data.low > I->high)
875  continue;
876 
877  if(data.high < I->low) {
878  line_point_data.insert(I, data);
879  break;
880  }
881 
882  // overlap
883 
884  I->low = (I->low < data.low) ? I->low : data.low;
885  I->high = (I->high > data.high) ? I->high : data.high;
886  I->cross = (I->cross != data.cross);
887 
888  auto J = I;
889 
890  ++J;
891 
892  if(J->low < I->high) {
893  I->high = J->high;
894  I->cross = (I->cross != J->cross);
895  line_point_data.erase(J);
896  }
897  }
898 
899  if(I == line_point_data.end())
900  line_point_data.push_back(data);
901 
902  old_below = below;
903  old_p = p_j;
904  continue;
905  }
906 
907  // the corner doesn't lie on the line, compute the intersection point
908 
909  bool below = (Cross(v, v_i) < 0);
910 
911  if(below != old_below) {
912  old_below = below;
913  Vector<2> dist = p - old_p;
914  CoordType dist_sqr_mag = dist.sqrMag();
915  CoordType dist_proj = Dot(dist, v);
916 
917  CoordType denom = dist_proj * dist_proj - dist_sqr_mag;
918 
919  assert(denom != 0); // We got a crossing, the vectors can't be parallel
920 
921  CoordType line_pos = (dist_proj * Dot(v_i, dist) + dist_sqr_mag * proj) / denom;
922 
923  cross[next_cross++] = line_pos;
924  }
925 
926  old_p = p;
927  }
928 
929  cross.resize(next_cross);
930  std::sort(cross.begin(), cross.end());
931 
932  if(!line_point_data.empty()) {
933  auto I = line_point_data.begin();
934  auto cross_num = cross.begin();
935  bool hit = false;
936 
937  while(cross_num != cross.end() && I != line_point_data.end()) {
938  if(*cross_num < I->low) {
939  ++cross_num;
940  hit = !hit;
941  continue;
942  }
943 
944  bool hit_between;
945  if(*cross_num > I->high) {
946  hit_between = I->cross;
947  }
948  else {
949  auto high_cross_num = cross_num;
950 
951  do {
952  ++high_cross_num;
953  } while(*high_cross_num < I->high);
954 
955  hit_between = (((high_cross_num - cross_num) % 2) != 0) != I->cross;
956 
957  cross_num = cross.erase(cross_num, high_cross_num);
958  }
959 
960  if(hit_between) {
961  cross_num = cross.insert(cross_num, proper == hit ? I->low : I->high);
962  ++cross_num;
963  hit = !hit;
964  }
965  else if(!proper) {
966  cross_num = cross.insert(cross_num, I->low);
967  ++cross_num;
968  cross_num = cross.insert(cross_num, I->high);
969  ++cross_num;
970  }
971  ++I;
972  }
973 
974  while(I != line_point_data.end()) {
975  if(I->cross) {
976  cross.push_back(proper == hit ? I->low : I->high);
977  hit = !hit;
978  }
979  else if(!proper) {
980  cross.push_back(I->low);
981  cross.push_back(I->high);
982  }
983  ++I;
984  }
985 
986  assert(!hit); // end outside the polygon
987  }
988 
989  return !cross.empty();
990 }
991 
992 bool PolyPolyIntersect(const Polygon<2> &poly1, const Polygon<2> &poly2,
993  const int intersect_dim,
994  const Poly2OrientIntersectData &data, bool proper)
995 {
996  switch(intersect_dim) {
997  case -1:
998  return false;
999  case 0:
1000  return Intersect(poly1, data.p1, proper)
1001  && Intersect(poly2, data.p2, proper);
1002  case 1:
1003  if(proper && (data.o1_is_line || data.o2_is_line))
1004  return false;
1005 
1006  if(data.o1_is_line && data.o2_is_line) {
1007  assert(!proper);
1008  CoordType low1, low2, high1, high2;
1009 
1010  LinePolyGetBounds(poly1, low1, high1);
1011 
1012  low1 -= data.p1[0];
1013  high1 -= data.p1[0];
1014 
1015  if(data.v1[0] < 0) { // v1 = (-1, 0)
1016  CoordType tmp = low1;
1017  low1 = -high1;
1018  high1 = -tmp;
1019  }
1020 
1021  LinePolyGetBounds(poly2, low2, high2);
1022 
1023  low2 -= data.p2[0];
1024  high2 -= data.p2[0];
1025 
1026  if(data.v2[0] < 0) { // v2 = (-1, 0)
1027  CoordType tmp = low2;
1028  low2 = -high2;
1029  high2 = -tmp;
1030  }
1031 
1032  return high1 >= low2 && high2 >= low1;
1033  }
1034 
1035  if(data.o1_is_line) {
1036  assert(!proper);
1037  CoordType min, max;
1038  LinePolyGetBounds(poly1, min, max);
1039 
1040  min -= data.p1[0];
1041  max -= data.p1[0];
1042 
1043  if(data.v1[0] < 0) { // v1 = (-1, 0)
1044  CoordType tmp = min;
1045  min = -max;
1046  max = -tmp;
1047  }
1048 
1049  Segment<2> s(data.p2 + min * data.v2, data.p1 + max * data.v2);
1050 
1051  return Intersect(poly2, s, false);
1052  }
1053 
1054  if(data.o2_is_line) {
1055  assert(!proper);
1056  CoordType min, max;
1057  LinePolyGetBounds(poly2, min, max);
1058 
1059  min -= data.p2[0];
1060  max -= data.p2[0];
1061 
1062  if(data.v2[0] < 0) { // v2 = (-1, 0)
1063  CoordType tmp = min;
1064  min = -max;
1065  max = -tmp;
1066  }
1067 
1068  Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
1069 
1070  return Intersect(poly1, s, false);
1071  }
1072 
1073  {
1074  std::vector<CoordType> cross1(poly1.numCorners());
1075  if(!GetCrossings(poly1, data.p1, data.v1, cross1, proper))
1076  return false; // line misses polygon
1077 
1078  std::vector<CoordType> cross2(poly2.numCorners());
1079  if(!GetCrossings(poly2, data.p2, data.v2, cross2, proper))
1080  return false; // line misses polygon
1081 
1082  auto i1 = cross1.begin(), i2 = cross2.begin();
1083  bool hit1 = false, hit2 = false;
1084 
1085  while(i1 != cross1.end() && i2 != cross2.end()) {
1086  if(Equal(*i1, *i2)) {
1087  if(!proper)
1088  return true;
1089 
1090  hit1 = !hit1;
1091  ++i1;
1092  hit2 = !hit2;
1093  ++i2;
1094  }
1095 
1096  if(*i1 < *i2) {
1097  hit1 = !hit1;
1098  ++i1;
1099  }
1100  else {
1101  hit2 = !hit2;
1102  ++i2;
1103  }
1104 
1105  if(hit1 && hit2)
1106  return true;
1107  }
1108 
1109  return false;
1110  }
1111  case 2:
1112  // Shift one polygon into the other's coordinates.
1113  // Perhaps not the most efficient, but this is a
1114  // rare special case.
1115  {
1116  Polygon<2> tmp_poly(poly2);
1117 
1118  for(size_t i = 0; i < tmp_poly.numCorners(); ++i) {
1119  Point<2> &p = tmp_poly[i];
1120  Point<2> shift_p = p + data.off;
1121 
1122  p[0] = shift_p[0] * data.v1[0] + shift_p[1] * data.v2[0];
1123  p[1] = shift_p[0] * data.v1[1] + shift_p[1] * data.v2[1];
1124  }
1125 
1126  return Intersect(poly1, tmp_poly, proper);
1127  }
1128  default:
1129  assert(false);
1130  return false;
1131  }
1132 }
1133 
1134 bool PolyPolyContains(const Polygon<2> &outer, const Polygon<2> &inner,
1135  const int intersect_dim,
1136  const Poly2OrientIntersectData &data, bool proper)
1137 {
1138  switch(intersect_dim) {
1139  case -1:
1140  return false;
1141  case 0:
1142  return Contains(data.p2, inner, false)
1143  && Contains(outer, data.p1, proper);
1144  case 1:
1145  if(!data.o2_is_line) // The inner poly isn't contained by the intersect line
1146  return false;
1147 
1148  // The inner poly lies on a line, so it reduces to a line segment
1149  {
1150  CoordType min, max;
1151  LinePolyGetBounds(inner, min, max);
1152 
1153  min -= data.p2[0];
1154  max -= data.p2[0];
1155 
1156  if(data.v2[0] < 0) { // v2 = (-1, 0)
1157  CoordType tmp = min;
1158  min = -max;
1159  max = -tmp;
1160  }
1161 
1162  Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
1163 
1164  return Contains(outer, s, proper);
1165  }
1166  case 2:
1167  // Shift one polygon into the other's coordinates.
1168  // Perhaps not the most efficient, but this is a
1169  // rare special case.
1170  {
1171  Polygon<2> tmp_poly(inner);
1172 
1173  for(size_t i = 0; i < tmp_poly.numCorners(); ++i) {
1174  Point<2> &p = tmp_poly[i];
1175  Point<2> shift_p = p + data.off;
1176 
1177  p[0] = shift_p[0] * data.v1[0] + shift_p[1] * data.v2[0];
1178  p[1] = shift_p[0] * data.v1[1] + shift_p[1] * data.v2[1];
1179  }
1180 
1181  return Contains(outer, tmp_poly, proper);
1182  }
1183  default:
1184  assert(false);
1185  return false;
1186  }
1187 }
1188 
1189 // Polygon<2> intersection functions
1190 
1191 // FIXME deal with round off error in _all_ these intersection functions
1192 
1193 // The Polygon<2>/Point<2> intersection function was stolen directly
1194 // from shape.cpp in libCoal
1195 
1196 template<>
1197 bool Intersect<2>(const Polygon<2>& r, const Point<2>& p, bool proper)
1198 {
1199  const Polygon<2>::theConstIter begin = r.m_points.begin(), end = r.m_points.end();
1200  bool hit = false;
1201 
1202  for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
1203  bool vertically_between =
1204  (((*i)[1] <= p[1] && p[1] < (*j)[1]) ||
1205  ((*j)[1] <= p[1] && p[1] < (*i)[1]));
1206 
1207  if (!vertically_between)
1208  continue;
1209 
1210  CoordType x_intersect = (*i)[0] + ((*j)[0] - (*i)[0])
1211  * (p[1] - (*i)[1]) / ((*j)[1] - (*i)[1]);
1212 
1213  if(Equal(p[0], x_intersect))
1214  return !proper;
1215 
1216  if(p[0] < x_intersect)
1217  hit = !hit;
1218  }
1219 
1220  return hit;
1221 }
1222 
1223 template<>
1224 bool Contains<2>(const Point<2>& p, const Polygon<2>& r, bool proper)
1225 {
1226  if(proper) // Weird degenerate case
1227  return r.numCorners() == 0;
1228 
1229  for(const auto & point : r.m_points)
1230  if(p != point)
1231  return false;
1232 
1233  return true;
1234 }
1235 
1236 template<>
1237 bool Intersect<2>(const Polygon<2>& p, const AxisBox<2>& b, bool proper)
1238 {
1239  const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1240  bool hit = false;
1241 
1242  for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
1243  bool low_vertically_between =
1244  (((*i)[1] <= b.m_low[1] && b.m_low[1] < (*j)[1]) ||
1245  ((*j)[1] <= b.m_low[1] && b.m_low[1] < (*i)[1]));
1246  bool low_horizontally_between =
1247  (((*i)[0] <= b.m_low[0] && b.m_low[0] < (*j)[0]) ||
1248  ((*j)[0] <= b.m_low[0] && b.m_low[0] < (*i)[0]));
1249  bool high_vertically_between =
1250  (((*i)[1] <= b.m_high[1] && b.m_high[1] < (*j)[1]) ||
1251  ((*j)[1] <= b.m_high[1] && b.m_high[1] < (*i)[1]));
1252  bool high_horizontally_between =
1253  (((*i)[0] <= b.m_high[0] && b.m_high[0] < (*j)[0]) ||
1254  ((*j)[0] <= b.m_high[0] && b.m_high[0] < (*i)[0]));
1255 
1256  CoordType xdiff = ((*j)[0] - (*i)[0]);
1257  CoordType ydiff = ((*j)[1] - (*i)[1]);
1258 
1259  if(low_vertically_between) { // Check for edge intersect
1260  CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
1261  * xdiff / ydiff;
1262 
1263  if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
1264  return !proper;
1265  if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1266  return true;
1267 
1268  // Also check for point inclusion here, only need to do this for one point
1269  if(b.m_low[0] < x_intersect)
1270  hit = !hit;
1271  }
1272 
1273  if(low_horizontally_between) { // Check for edge intersect
1274  CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
1275  * ydiff / xdiff;
1276 
1277  if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
1278  return !proper;
1279  if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1280  return true;
1281  }
1282 
1283  if(high_vertically_between) { // Check for edge intersect
1284  CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
1285  * xdiff / ydiff;
1286 
1287  if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
1288  return !proper;
1289  if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1290  return true;
1291  }
1292 
1293  if(high_horizontally_between) { // Check for edge intersect
1294  CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
1295  * ydiff / xdiff;
1296 
1297  if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
1298  return !proper;
1299  if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1300  return true;
1301  }
1302  }
1303 
1304  return hit;
1305 }
1306 
1307 template<>
1308 bool Contains<2>(const Polygon<2>& p, const AxisBox<2>& b, bool proper)
1309 {
1310  const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1311  bool hit = false;
1312 
1313  for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
1314  bool low_vertically_between =
1315  (((*i)[1] <= b.m_low[1] && b.m_low[1] < (*j)[1]) ||
1316  ((*j)[1] <= b.m_low[1] && b.m_low[1] < (*i)[1]));
1317  bool low_horizontally_between =
1318  (((*i)[0] <= b.m_low[0] && b.m_low[0] < (*j)[0]) ||
1319  ((*j)[0] <= b.m_low[0] && b.m_low[0] < (*i)[0]));
1320  bool high_vertically_between =
1321  (((*i)[1] <= b.m_high[1] && b.m_high[1] < (*j)[1]) ||
1322  ((*j)[1] <= b.m_high[1] && b.m_high[1] < (*i)[1]));
1323  bool high_horizontally_between =
1324  (((*i)[0] <= b.m_high[0] && b.m_high[0] < (*j)[0]) ||
1325  ((*j)[0] <= b.m_high[0] && b.m_high[0] < (*i)[0]));
1326 
1327  CoordType xdiff = ((*j)[0] - (*i)[0]);
1328  CoordType ydiff = ((*j)[1] - (*i)[1]);
1329 
1330  if(low_vertically_between) { // Check for edge intersect
1331  CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
1332  * xdiff / ydiff;
1333 
1334  bool on_corner = Equal(b.m_low[0], x_intersect)
1335  || Equal(b.m_high[0], x_intersect);
1336 
1337  if(on_corner && proper)
1338  return false;
1339 
1340  if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1341  return false;
1342 
1343  // Also check for point inclusion here, only need to do this for one point
1344  if(!on_corner && b.m_low[0] < x_intersect)
1345  hit = !hit;
1346  }
1347 
1348  if(low_horizontally_between) { // Check for edge intersect
1349  CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
1350  * ydiff / xdiff;
1351 
1352  bool on_corner = Equal(b.m_low[1], y_intersect)
1353  || Equal(b.m_high[1], y_intersect);
1354 
1355  if(on_corner && proper)
1356  return false;
1357 
1358  if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1359  return false;
1360  }
1361 
1362  if(high_vertically_between) { // Check for edge intersect
1363  CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
1364  * xdiff / ydiff;
1365 
1366  bool on_corner = Equal(b.m_low[0], x_intersect)
1367  || Equal(b.m_high[0], x_intersect);
1368 
1369  if(on_corner && proper)
1370  return false;
1371 
1372  if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1373  return false;
1374  }
1375 
1376  if(high_horizontally_between) { // Check for edge intersect
1377  CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
1378  * ydiff / xdiff;
1379 
1380  bool on_corner = Equal(b.m_low[1], y_intersect)
1381  || Equal(b.m_high[1], y_intersect);
1382 
1383  if(on_corner && proper)
1384  return false;
1385 
1386  if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1387  return false;
1388  }
1389  }
1390 
1391  return hit;
1392 }
1393 
1394 template<>
1395 bool Contains<2>(const AxisBox<2>& b, const Polygon<2>& p, bool proper)
1396 {
1397  for(const auto & point : p.m_points)
1398  if(!Contains(b, point, proper))
1399  return false;
1400 
1401  return true;
1402 }
1403 
1404 template<>
1405 bool Intersect<2>(const Polygon<2>& p, const Ball<2>& b, bool proper)
1406 {
1407  if(Contains(p, b.m_center, proper))
1408  return true;
1409 
1410  Segment<2> s2;
1411  s2.endpoint(0) = p.m_points.back();
1412  int next_end = 1;
1413 
1414  for(const auto & point : p.m_points) {
1415  s2.endpoint(next_end) = point;
1416  if(Intersect(s2, b, proper))
1417  return true;
1418  next_end = next_end ? 0 : 1;
1419  }
1420 
1421  return false;
1422 }
1423 
1424 template<>
1425 bool Contains<2>(const Polygon<2>& p, const Ball<2>& b, bool proper)
1426 {
1427  if(!Contains(p, b.m_center, proper))
1428  return false;
1429 
1430  Segment<2> s2;
1431  s2.endpoint(0) = p.m_points.back();
1432  int next_end = 1;
1433 
1434  for(const auto & point : p.m_points) {
1435  s2.endpoint(next_end) = point;
1436  if(Intersect(s2, b, !proper))
1437  return false;
1438  next_end = next_end ? 0 : 1;
1439  }
1440 
1441  return true;
1442 }
1443 
1444 template<>
1445 bool Contains<2>(const Ball<2>& b, const Polygon<2>& p, bool proper)
1446 {
1447  CoordType sqr_dist = b.m_radius * b.m_radius;
1448 
1449  for(const auto & point : p.m_points)
1450  if(_Greater(SquaredDistance(b.m_center, point), sqr_dist, proper))
1451  return false;
1452 
1453  return true;
1454 }
1455 
1456 template<>
1457 bool Intersect<2>(const Polygon<2>& p, const Segment<2>& s, bool proper)
1458 {
1459  if(Contains(p, s.endpoint(0), proper))
1460  return true;
1461 
1462  const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1463 
1464  Segment<2> s2;
1465 
1466  s2.endpoint(0) = p.m_points.back();
1467  int next_point = 1;
1468 
1469  for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1470  s2.endpoint(next_point) = *i;
1471  if(Intersect(s, s2, proper))
1472  return true;
1473  next_point = next_point ? 0 : 1;
1474  }
1475 
1476  return false;
1477 }
1478 
1479 template<>
1480 bool Contains<2>(const Polygon<2>& p, const Segment<2>& s, bool proper)
1481 {
1482  if(proper && !Contains(p, s.endpoint(0), true))
1483  return false;
1484 
1485  const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1486 
1487  Segment<2> s2;
1488 
1489  s2.endpoint(0) = p.m_points.back();
1490  int next_point = 1;
1491  bool hit = false;
1492 
1493  for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1494  s2.endpoint(next_point) = *i;
1495  if(Intersect(s2, s, !proper))
1496  return false;
1497  bool this_point = next_point;
1498  next_point = next_point ? 0 : 1;
1499  if(proper)
1500  continue;
1501 
1502  // Check for crossing at an endpoint
1503  if(Contains(s, *i, false) && (*i != s.m_p2)) {
1504  Vector<2> segment = s.m_p2 - s.m_p1;
1505  Vector<2> edge1 = *i - s2.endpoint(next_point); // Gives prev point in this case
1506  Vector<2> edge2 = *i - *(i + 1);
1507 
1508  CoordType c1 = Cross(segment, edge1), c2 = Cross(segment, edge2);
1509 
1510  if(c1 * c2 < 0) { // opposite sides
1511  if(*i == s.m_p1) { // really a containment issue
1512  if(edge1[1] * edge2[1] > 0 // Edges either both up or both down
1513  || ((edge1[1] > 0) ? c1 : c2) < 0) // segment lies to the left
1514  hit = !hit;
1515  continue; // Already checked containment for this point
1516  }
1517  else
1518  return false;
1519  }
1520  }
1521 
1522  // Check containment of one endpoint
1523 
1524  // next_point also gives prev_point
1525  bool vertically_between =
1526  ((s2.endpoint(this_point)[1] <= s.m_p1[1]
1527  && s.m_p1[1] < s2.endpoint(next_point)[1]) ||
1528  (s2.endpoint(next_point)[1] <= s.m_p1[1]
1529  && s.m_p1[1] < s2.endpoint(this_point)[1]));
1530 
1531  if (!vertically_between)
1532  continue;
1533 
1534  CoordType x_intersect = s2.m_p1[0] + (s2.m_p2[0] - s2.m_p1[0])
1535  * (s.m_p1[1] - s2.m_p1[1])
1536  / (s2.m_p2[1] - s2.m_p1[1]);
1537 
1538  if(Equal(s.m_p1[0], x_intersect)) { // Figure out which side the segment's on
1539 
1540  // Equal points are handled in the crossing routine above
1541  if(s2.endpoint(next_point) == s.m_p1)
1542  continue;
1543  assert(s2.endpoint(this_point) != s.m_p1);
1544 
1545  Vector<2> poly_edge = (s2.m_p1[1] < s2.m_p2[1]) ? (s2.m_p2 - s2.m_p1)
1546  : (s2.m_p1 - s2.m_p2);
1547  Vector<2> segment = s.m_p2 - s.m_p1;
1548 
1549  if(Cross(segment, poly_edge) < 0)
1550  hit = !hit;
1551  }
1552  else if(s.m_p1[0] < x_intersect)
1553  hit = !hit;
1554  }
1555 
1556  return proper || hit;
1557 }
1558 
1559 template<>
1560 bool Contains<2>(const Segment<2>& s, const Polygon<2>& p, bool proper)
1561 {
1562  for(const auto & point : p.m_points)
1563  if(!Contains(s, point, proper))
1564  return false;
1565 
1566  return true;
1567 }
1568 
1569 template<>
1570 bool Intersect<2>(const Polygon<2>& p, const RotBox<2>& r, bool proper)
1571 {
1572  CoordType m_low[2], m_high[2];
1573 
1574  for(int j = 0; j < 2; ++j) {
1575  if(r.m_size[j] > 0) {
1576  m_low[j] = r.m_corner0[j];
1577  m_high[j] = r.m_corner0[j] + r.m_size[j];
1578  }
1579  else {
1580  m_high[j] = r.m_corner0[j];
1581  m_low[j] = r.m_corner0[j] + r.m_size[j];
1582  }
1583  }
1584 
1585  Point<2> ends[2];
1586  ends[0] = p.m_points.back();
1587  // FIXME Point<>::rotateInverse()
1588  ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1589  int next_end = 1;
1590 
1591  const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1592  bool hit = false;
1593 
1594  for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1595  ends[next_end] = *i;
1596  // FIXME Point<>::rotateInverse()
1597  ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1598  next_end = next_end ? 0 : 1;
1599 
1600  bool low_vertically_between =
1601  (((ends[0])[1] <= m_low[1] && m_low[1] < (ends[1])[1]) ||
1602  ((ends[1])[1] <= m_low[1] && m_low[1] < (ends[0])[1]));
1603  bool low_horizontally_between =
1604  (((ends[0])[0] <= m_low[0] && m_low[0] < (ends[1])[0]) ||
1605  ((ends[1])[0] <= m_low[0] && m_low[0] < (ends[0])[0]));
1606  bool high_vertically_between =
1607  (((ends[0])[1] <= m_high[1] && m_high[1] < (ends[1])[1]) ||
1608  ((ends[1])[1] <= m_high[1] && m_high[1] < (ends[0])[1]));
1609  bool high_horizontally_between =
1610  (((ends[0])[0] <= m_high[0] && m_high[0] < (ends[1])[0]) ||
1611  ((ends[1])[0] <= m_high[0] && m_high[0] < (ends[0])[0]));
1612 
1613  CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1614  CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1615 
1616  if(low_vertically_between) { // Check for edge intersect
1617  CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1618  * xdiff / ydiff;
1619 
1620  if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
1621  return !proper;
1622  if(m_low[0] < x_intersect && m_high[0] > x_intersect)
1623  return true;
1624 
1625  // Also check for point inclusion here, only need to do this for one point
1626  if(m_low[0] < x_intersect)
1627  hit = !hit;
1628  }
1629 
1630  if(low_horizontally_between) { // Check for edge intersect
1631  CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1632  * ydiff / xdiff;
1633 
1634  if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1635  return !proper;
1636  if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1637  return true;
1638  }
1639 
1640  if(high_vertically_between) { // Check for edge intersect
1641  CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1642  * xdiff / ydiff;
1643 
1644  if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
1645  return !proper;
1646  if(m_low[0] < x_intersect && m_high[0] > x_intersect)
1647  return true;
1648  }
1649 
1650  if(high_horizontally_between) { // Check for edge intersect
1651  CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1652  * ydiff / xdiff;
1653 
1654  if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1655  return !proper;
1656  if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1657  return true;
1658  }
1659  }
1660 
1661  return hit;
1662 }
1663 
1664 template<>
1665 bool Contains<2>(const Polygon<2>& p, const RotBox<2>& r, bool proper)
1666 {
1667  CoordType m_low[2], m_high[2];
1668 
1669  for(int j = 0; j < 2; ++j) {
1670  if(r.m_size[j] > 0) {
1671  m_low[j] = r.m_corner0[j];
1672  m_high[j] = r.m_corner0[j] + r.m_size[j];
1673  }
1674  else {
1675  m_high[j] = r.m_corner0[j];
1676  m_low[j] = r.m_corner0[j] + r.m_size[j];
1677  }
1678  }
1679 
1680  Point<2> ends[2];
1681  ends[0] = p.m_points.back();
1682  // FIXME Point<>::rotateInverse()
1683  ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1684  int next_end = 1;
1685 
1686  const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1687  bool hit = false;
1688 
1689  for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1690  ends[next_end] = *i;
1691  // FIXME Point<>::rotateInverse()
1692  ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1693  next_end = next_end ? 0 : 1;
1694 
1695  bool low_vertically_between =
1696  (((ends[0])[1] <= m_low[1] && m_low[1] < (ends[1])[1]) ||
1697  ((ends[1])[1] <= m_low[1] && m_low[1] < (ends[0])[1]));
1698  bool low_horizontally_between =
1699  (((ends[0])[0] <= m_low[0] && m_low[0] < (ends[1])[0]) ||
1700  ((ends[1])[0] <= m_low[0] && m_low[0] < (ends[0])[0]));
1701  bool high_vertically_between =
1702  (((ends[0])[1] <= m_high[1] && m_high[1] < (ends[1])[1]) ||
1703  ((ends[1])[1] <= m_high[1] && m_high[1] < (ends[0])[1]));
1704  bool high_horizontally_between =
1705  (((ends[0])[0] <= m_high[0] && m_high[0] < (ends[1])[0]) ||
1706  ((ends[1])[0] <= m_high[0] && m_high[0] < (ends[0])[0]));
1707 
1708  CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1709  CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1710 
1711  if(low_vertically_between) { // Check for edge intersect
1712  CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1713  * xdiff / ydiff;
1714 
1715  bool on_corner = Equal(m_low[0], x_intersect)
1716  || Equal(m_high[0], x_intersect);
1717 
1718  if(on_corner && proper)
1719  return false;
1720 
1721  if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1722  return false;
1723 
1724  // Also check for point inclusion here, only need to do this for one point
1725  if(!on_corner && m_low[0] < x_intersect)
1726  hit = !hit;
1727  }
1728 
1729  if(low_horizontally_between) { // Check for edge intersect
1730  CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1731  * ydiff / xdiff;
1732 
1733  bool on_corner = Equal(m_low[1], y_intersect)
1734  || Equal(m_high[1], y_intersect);
1735 
1736  if(on_corner && proper)
1737  return false;
1738 
1739  if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1740  return false;
1741  }
1742 
1743  if(high_vertically_between) { // Check for edge intersect
1744  CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1745  * xdiff / ydiff;
1746 
1747  bool on_corner = Equal(m_low[0], x_intersect)
1748  || Equal(m_high[0], x_intersect);
1749 
1750  if(on_corner && proper)
1751  return false;
1752 
1753  if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1754  return false;
1755  }
1756 
1757  if(high_horizontally_between) { // Check for edge intersect
1758  CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1759  * ydiff / xdiff;
1760 
1761  bool on_corner = Equal(m_low[1], y_intersect)
1762  || Equal(m_high[1], y_intersect);
1763 
1764  if(on_corner && proper)
1765  return false;
1766 
1767  if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1768  return false;
1769  }
1770  }
1771 
1772  return hit;
1773 }
1774 
1775 template<>
1776 bool Contains<2>(const RotBox<2>& r, const Polygon<2>& p, bool proper)
1777 {
1778  for(const auto & point : p.m_points)
1779  if(!Contains(r, point, proper))
1780  return false;
1781 
1782  return true;
1783 }
1784 
1785 template<>
1786 bool Intersect<2>(const Polygon<2>& p1, const Polygon<2>& p2, bool proper)
1787 {
1788  auto begin1 = p1.m_points.begin(), end1 = p1.m_points.end();
1789  auto begin2 = p2.m_points.begin(), end2 = p2.m_points.end();
1790  Segment<2> s1, s2;
1791  int next_end1, next_end2;
1792 
1793  s1.endpoint(0) = p1.m_points.back();
1794  s2.endpoint(0) = p2.m_points.back();
1795  next_end1 = next_end2 = 1;
1796  for(auto i1 = begin1; i1 != end1; ++i1) {
1797  s1.endpoint(next_end1) = *i1;
1798  next_end1 = next_end1 ? 0 : 1;
1799 
1800  for(auto i2 = begin2; i2 != end2; ++i2) {
1801  s2.endpoint(next_end2) = *i2;
1802  next_end2 = next_end2 ? 0 : 1;
1803 
1804  if(Intersect(s1, s2, proper))
1805  return true;
1806  }
1807  }
1808 
1809  return Contains(p1, p2.m_points.front(), proper)
1810  || Contains(p2, p1.m_points.front(), proper);
1811 }
1812 
1813 template<>
1814 bool Contains<2>(const Polygon<2>& outer, const Polygon<2>& inner, bool proper)
1815 {
1816  if(proper && !Contains(outer, inner.m_points.front(), true))
1817  return false;
1818 
1819  auto begin = inner.m_points.begin(), end = inner.m_points.end();
1820  Segment<2> s;
1821  s.endpoint(0) = inner.m_points.back();
1822  int next_end = 1;
1823 
1824  for(auto i = begin; i != end; ++i) {
1825  s.endpoint(next_end) = *i;
1826  next_end = next_end ? 0 : 1;
1827  if(!proper) {
1828  if(!Contains(outer, s, false))
1829  return false;
1830  }
1831  else {
1832  auto begin2 = outer.m_points.begin(),
1833  end2 = outer.m_points.end();
1834  Segment<2> s2;
1835  s2.endpoint(0) = outer.m_points.back();
1836  int next_end2 = 1;
1837  for(auto i2 = begin2; i2 != end2; ++i2) {
1838  s2.endpoint(next_end2) = *i2;
1839  next_end2 = next_end2 ? 0 : 1;
1840 
1841  if(Intersect(s, s2, false))
1842  return false;
1843  }
1844  }
1845  }
1846 
1847  return true;
1848 }
1849 
1850 }
The 2D specialization of the Polygon<> template.
Definition: polygon.h:48
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:218
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
Definition: vector.cpp:102
Point< dim > Midpoint(const Point< dim > &p1, const Point< dim > &p2, CoordType dist=0.5)
Definition: point_funcs.h:219
RotMatrix< dim > ProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2^-1
static FloatType epsilon()
This is the attempted precision of the library.