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