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
34namespace WFMath {
35
36
37
38template<int dim>
39inline 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
53template<int dim>
54inline 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
65template<>
66bool Poly2Orient<3>::checkIntersectPlane(const AxisBox<3>& b, Point<2>& p2,
67 bool proper) const;
68
69template<int dim>
70bool 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
137template<int dim>
138int 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
343template<int dim>
344inline 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
352template<int dim>
353inline 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
370template<int dim>
371bool 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
397template<int dim>
398bool _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
454template<int dim>
455inline 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
460template<int dim>
461inline 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
470template<int dim>
471inline 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
487template<int dim>
488inline 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
504template<int dim>
505inline 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
525template<int dim>
526bool 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
554template<int dim>
555inline 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
570template<int dim>
571inline 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
589template<int dim>
590bool 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
622template<int dim>
623inline 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
631template<int dim>
632inline 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
649bool PolyPolyIntersect(const Polygon<2> &poly1, const Polygon<2> &poly2,
650 int intersect_dim,
651 const Poly2OrientIntersectData &data, bool proper);
652
653template<int dim>
654inline 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
663bool PolyPolyContains(const Polygon<2> &outer, const Polygon<2> &inner,
664 int intersect_dim,
665 const Poly2OrientIntersectData &data, bool proper);
666
667template<int dim>
668inline 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
686template bool Intersect<Point<2>,Polygon<2> >(const Point<2>&, const Polygon<2>&, bool);
687template bool Intersect<Point<3>,Polygon<3> >(const Point<3>&, const Polygon<3>&, bool);
688template bool Contains<3>(const Point<3>&, const Polygon<3>&, bool);
689template bool Intersect<3>(const Polygon<3>&, const Point<3>&, bool);
690template bool Contains<3>(const Polygon<3>&, const Point<3>&, bool);
691
692template bool Intersect<AxisBox<2>,Polygon<2> >(const AxisBox<2>&, const Polygon<2>&, bool);
693template bool Intersect<AxisBox<3>,Polygon<3> >(const AxisBox<3>&, const Polygon<3>&, bool);
694template bool Contains<3>(const AxisBox<3>&, const Polygon<3>&, bool);
695template bool Intersect<3>(const Polygon<3>&, const AxisBox<3>&, bool);
696template bool Contains<3>(const Polygon<3>&, const AxisBox<3>&, bool);
697
698template bool Intersect<Ball<2>,Polygon<2> >(const Ball<2>&, const Polygon<2>&, bool);
699template bool Intersect<Ball<3>,Polygon<3> >(const Ball<3>&, const Polygon<3>&, bool);
700template bool Contains<3>(const Ball<3>&, const Polygon<3>&, bool);
701template bool Intersect<3>(const Polygon<3>&, const Ball<3>&, bool);
702template bool Contains<3>(const Polygon<3>&, const Ball<3>&, bool);
703
704template bool Intersect<Segment<2>,Polygon<2> >(const Segment<2>&, const Polygon<2>&, bool);
705template bool Intersect<Segment<3>,Polygon<3> >(const Segment<3>&, const Polygon<3>&, bool);
706template bool Contains<3>(const Segment<3>&, const Polygon<3>&, bool);
707template bool Intersect<3>(const Polygon<3>&, const Segment<3>&, bool);
708template bool Contains<3>(const Polygon<3>&, const Segment<3>&, bool);
709
710template bool Intersect<RotBox<2>,Polygon<2> >(const RotBox<2>&, const Polygon<2>&, bool);
711template bool Intersect<RotBox<3>,Polygon<3> >(const RotBox<3>&, const Polygon<3>&, bool);
712template bool Contains<3>(const RotBox<3>&, const Polygon<3>&, bool);
713template bool Intersect<3>(const Polygon<3>&, const RotBox<3>&, bool);
714template bool Contains<3>(const Polygon<3>&, const RotBox<3>&, bool);
715
716template bool Intersect<3>(const Polygon<3>&, const Polygon<3>&, bool);
717template bool Contains<3>(const Polygon<3>&, const Polygon<3>&, bool);
718
719template<>
720bool 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
780static 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"
802static 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
991bool 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
1133bool 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
1195template<>
1196bool 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
1222template<>
1223bool 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
1235template<>
1236bool 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
1306template<>
1307bool 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
1393template<>
1394bool 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
1403template<>
1404bool 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
1423template<>
1424bool 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
1443template<>
1444bool 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
1455template<>
1456bool 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
1478template<>
1479bool 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
1558template<>
1559bool 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
1568template<>
1569bool 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
1663template<>
1664bool 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
1774template<>
1775bool 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
1784template<>
1785bool 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
1812template<>
1813bool 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.