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., 675 Mass Ave, Cambridge, MA 02139, 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 // instantiations, only need 3d because 2d is a specialization,
37 // except for the reverse-order intersect
38
39 template bool Intersect<Point<2>,Polygon<2> >(const Point<2>&, const Polygon<2>&, bool);
40 template bool Intersect<Point<3>,Polygon<3> >(const Point<3>&, const Polygon<3>&, bool);
41 template bool Contains<3>(const Point<3>&, const Polygon<3>&, bool);
42 template bool Intersect<3>(const Polygon<3>&, const Point<3>&, bool);
43 template bool Contains<3>(const Polygon<3>&, const Point<3>&, bool);
44
45 template bool Intersect<AxisBox<2>,Polygon<2> >(const AxisBox<2>&, const Polygon<2>&, bool);
46 template bool Intersect<AxisBox<3>,Polygon<3> >(const AxisBox<3>&, const Polygon<3>&, bool);
47 template bool Contains<3>(const AxisBox<3>&, const Polygon<3>&, bool);
48 template bool Intersect<3>(const Polygon<3>&, const AxisBox<3>&, bool);
49 template bool Contains<3>(const Polygon<3>&, const AxisBox<3>&, bool);
50
51 template bool Intersect<Ball<2>,Polygon<2> >(const Ball<2>&, const Polygon<2>&, bool);
52 template bool Intersect<Ball<3>,Polygon<3> >(const Ball<3>&, const Polygon<3>&, bool);
53 template bool Contains<3>(const Ball<3>&, const Polygon<3>&, bool);
54 template bool Intersect<3>(const Polygon<3>&, const Ball<3>&, bool);
55 template bool Contains<3>(const Polygon<3>&, const Ball<3>&, bool);
56
57 template bool Intersect<Segment<2>,Polygon<2> >(const Segment<2>&, const Polygon<2>&, bool);
58 template bool Intersect<Segment<3>,Polygon<3> >(const Segment<3>&, const Polygon<3>&, bool);
59 template bool Contains<3>(const Segment<3>&, const Polygon<3>&, bool);
60 template bool Intersect<3>(const Polygon<3>&, const Segment<3>&, bool);
61 template bool Contains<3>(const Polygon<3>&, const Segment<3>&, bool);
62
63 template bool Intersect<RotBox<2>,Polygon<2> >(const RotBox<2>&, const Polygon<2>&, bool);
64 template bool Intersect<RotBox<3>,Polygon<3> >(const RotBox<3>&, const Polygon<3>&, bool);
65 template bool Contains<3>(const RotBox<3>&, const Polygon<3>&, bool);
66 template bool Intersect<3>(const Polygon<3>&, const RotBox<3>&, bool);
67 template bool Contains<3>(const Polygon<3>&, const RotBox<3>&, bool);
68
69 template bool Intersect<3>(const Polygon<3>&, const Polygon<3>&, bool);
70 template bool Contains<3>(const Polygon<3>&, const Polygon<3>&, bool);
71
72 template<>
checkIntersectPlane(const AxisBox<3> & b,Point<2> & p2,bool proper) const73 bool _Poly2Orient<3>::checkIntersectPlane(const AxisBox<3>& b, Point<2>& p2,
74 bool proper) const
75 {
76 assert("This function should only be called if the orientation represents a plane" &&
77 m_origin.isValid() && m_axes[0].isValid() && m_axes[1].isValid());
78
79 Vector<3> normal = Cross(m_axes[0], m_axes[1]); // normal to the plane
80
81 enum {
82 AXIS_UP,
83 AXIS_DOWN,
84 AXIS_FLAT
85 } axis_direction[3];
86
87 CoordType normal_mag = normal.sloppyMag();
88 int high_corner_num = 0;
89
90 for(int i = 0; i < 3; ++i) {
91 if(std::fabs(normal[i]) < normal_mag * numeric_constants<CoordType>::epsilon())
92 axis_direction[i] = AXIS_FLAT;
93 else if(normal[i] > 0) {
94 axis_direction[i] = AXIS_UP;
95 high_corner_num |= (1 << i);
96 }
97 else
98 axis_direction[i] = AXIS_DOWN;
99 }
100
101 int low_corner_num = high_corner_num ^ 7;
102
103 Point<3> high_corner = b.getCorner(high_corner_num);
104 Point<3> low_corner = b.getCorner(low_corner_num);
105
106 // If these are on opposite sides of the plane, we have an intersection
107
108 CoordType perp_size = Dot(normal, high_corner - low_corner) / normal_mag;
109 assert(perp_size >= 0);
110
111 if(perp_size < normal_mag * numeric_constants<CoordType>::epsilon()) {
112 // We have a very flat box, lying parallel to the plane
113 return !proper && checkContained(Midpoint(high_corner, low_corner), p2);
114 }
115
116 if(_Less(Dot(high_corner - m_origin, normal), 0, proper)
117 || _Less(Dot(low_corner - m_origin, normal), 0, proper))
118 return false; // box lies above or below the plane
119
120 // Find the intersection of the line through the corners with the plane
121
122 Point<2> p2_high, p2_low;
123
124 CoordType high_dist = offset(high_corner, p2_high).mag();
125 CoordType low_dist = offset(low_corner, p2_low).mag();
126
127 p2 = Midpoint(p2_high, p2_low, high_dist / (high_dist + low_dist));
128
129 return true;
130 }
131
132 // This assumes the y coordinates of the points are all zero
_LinePolyGetBounds(const Polygon<2> & poly,CoordType & low,CoordType & high)133 static void _LinePolyGetBounds(const Polygon<2> &poly, CoordType &low, CoordType &high)
134 {
135 low = high = poly[0][0];
136
137 for(size_t i = 0; i < poly.numCorners(); ++i) {
138 CoordType val = poly[i][0];
139 if(val < low)
140 low = val;
141 if(val > high)
142 high = val;
143 }
144 }
145
146 // For use in _GetCrossings()
147 struct LinePointData {
148 CoordType low, high;
149 bool cross;
150 };
151
152 // This finds the intervals where the polygon intersects the line
153 // through p parallel to v, and puts the endpoints of those
154 // intervals in the vector "cross"
_GetCrossings(const Polygon<2> & poly,const Point<2> & p,const Vector<2> & v,std::vector<CoordType> & cross,bool proper)155 static bool _GetCrossings(const Polygon<2> &poly, const Point<2> &p,
156 const Vector<2> &v, std::vector<CoordType> &cross,
157 bool proper)
158 {
159 assert(poly.numCorners() == cross.size()); // Already allocated
160 assert(Equal(v.sqrMag(), 1));
161
162 // The sign of the cross product changes when you cross the line
163 Point<2> old_p = poly.getCorner(poly.numCorners() - 1);
164 bool old_below = (Cross(v, old_p - p) < 0);
165 int next_cross = 0;
166
167 // Stuff for when multiple sequential corners lie on the line
168 std::list<LinePointData> line_point_data;
169
170 for(size_t i = 0; i < poly.numCorners(); ++i) {
171 Point<2> p_i = poly.getCorner(i);
172 Vector<2> v_i = p_i - p;
173
174 CoordType v_i_sqr_mag = v_i.sqrMag(), proj = Dot(v_i, v);
175
176 if(Equal(v_i_sqr_mag, proj * proj)) { // corner lies on line
177 Point<2> p_j;
178 Vector<2> v_j;
179 CoordType proj_j, low_proj = proj, high_proj = proj;
180 size_t j;
181 for(j = i + 1; j != i; j == poly.numCorners() - 1 ? j = 0 : ++j) {
182 p_j = poly.getCorner(j);
183 v_j = p_j - p;
184 proj_j = Dot(v_j, v);
185
186 if(!Equal(v_j.sqrMag(), proj_j * proj_j))
187 break;
188
189 if(proj_j < low_proj)
190 low_proj = proj_j;
191 if(proj_j > high_proj)
192 high_proj = proj_j;
193 }
194
195 assert(j != i); // We know that the polygon spans a 2d space
196
197 bool below = (Cross(v, v_j) < 0);
198
199 if(below == old_below && proper) {
200 old_p = p_j;
201 continue;
202 }
203
204 if(j == i + 1) { // just one point on the line
205
206 if(below != old_below) {
207 old_below = below;
208 cross[next_cross++] = proj;
209 }
210 else {
211 assert(!proper);
212 // Just touches, adding it twice will give a zero length "hit" region
213 cross[next_cross++] = proj;
214 cross[next_cross++] = proj;
215 }
216
217 old_p = p_j;
218 continue;
219 }
220
221 LinePointData data = {low_proj, high_proj, below != old_below};
222
223 std::list<LinePointData>::iterator I;
224
225 for(I = line_point_data.begin(); I != line_point_data.end(); ++I) {
226 if(data.low > I->high)
227 continue;
228
229 if(data.high < I->low) {
230 line_point_data.insert(I, data);
231 break;
232 }
233
234 // overlap
235
236 I->low = (I->low < data.low) ? I->low : data.low;
237 I->high = (I->high > data.high) ? I->high : data.high;
238 I->cross = (I->cross != data.cross);
239
240 std::list<LinePointData>::iterator J = I;
241
242 ++J;
243
244 if(J->low < I->high) {
245 I->high = J->high;
246 I->cross = (I->cross != J->cross);
247 line_point_data.erase(J);
248 }
249 }
250
251 if(I == line_point_data.end())
252 line_point_data.push_back(data);
253
254 old_below = below;
255 old_p = p_j;
256 continue;
257 }
258
259 // the corner doesn't lie on the line, compute the intersection point
260
261 bool below = (Cross(v, v_i) < 0);
262
263 if(below != old_below) {
264 old_below = below;
265 Vector<2> dist = p - old_p;
266 CoordType dist_sqr_mag = dist.sqrMag();
267 CoordType dist_proj = Dot(dist, v);
268
269 CoordType denom = dist_proj * dist_proj - dist_sqr_mag;
270
271 assert(denom != 0); // We got a crossing, the vectors can't be parallel
272
273 CoordType line_pos = (dist_proj * Dot(v_i, dist) + dist_sqr_mag * proj) / denom;
274
275 cross[next_cross++] = line_pos;
276 }
277
278 old_p = p;
279 }
280
281 cross.resize(next_cross);
282 std::sort(cross.begin(), cross.end());
283
284 if(!line_point_data.empty()) {
285 std::list<LinePointData>::iterator I = line_point_data.begin();
286 std::vector<CoordType>::iterator cross_num = cross.begin();
287 bool hit = false;
288
289 while(cross_num != cross.end() && I != line_point_data.end()) {
290 if(*cross_num < I->low) {
291 ++cross_num;
292 hit = !hit;
293 continue;
294 }
295
296 bool hit_between;
297 if(*cross_num > I->high) {
298 hit_between = I->cross;
299 }
300 else {
301 std::vector<CoordType>::iterator high_cross_num = cross_num;
302
303 do {
304 ++high_cross_num;
305 } while(*high_cross_num < I->high);
306
307 hit_between = (((high_cross_num - cross_num) % 2) != 0) != I->cross;
308
309 cross_num = cross.erase(cross_num, high_cross_num);
310 }
311
312 if(hit_between) {
313 cross_num = cross.insert(cross_num, proper == hit ? I->low : I->high);
314 ++cross_num;
315 hit = !hit;
316 }
317 else if(!proper) {
318 cross_num = cross.insert(cross_num, I->low);
319 ++cross_num;
320 cross_num = cross.insert(cross_num, I->high);
321 ++cross_num;
322 }
323 ++I;
324 }
325
326 while(I != line_point_data.end()) {
327 if(I->cross) {
328 cross.push_back(proper == hit ? I->low : I->high);
329 hit = !hit;
330 }
331 else if(!proper) {
332 cross.push_back(I->low);
333 cross.push_back(I->high);
334 }
335 ++I;
336 }
337
338 assert(!hit); // end outside the polygon
339 }
340
341 return cross.size() != 0;
342 }
343
_PolyPolyIntersect(const Polygon<2> & poly1,const Polygon<2> & poly2,const int intersect_dim,const _Poly2OrientIntersectData & data,bool proper)344 bool _PolyPolyIntersect(const Polygon<2> &poly1, const Polygon<2> &poly2,
345 const int intersect_dim,
346 const _Poly2OrientIntersectData &data, bool proper)
347 {
348 switch(intersect_dim) {
349 case -1:
350 return false;
351 case 0:
352 return Intersect(poly1, data.p1, proper)
353 && Intersect(poly2, data.p2, proper);
354 case 1:
355 if(proper && (data.o1_is_line || data.o2_is_line))
356 return false;
357
358 if(data.o1_is_line && data.o2_is_line) {
359 assert(!proper);
360 CoordType low1, low2, high1, high2;
361
362 _LinePolyGetBounds(poly1, low1, high1);
363
364 low1 -= data.p1[0];
365 high1 -= data.p1[0];
366
367 if(data.v1[0] < 0) { // v1 = (-1, 0)
368 CoordType tmp = low1;
369 low1 = -high1;
370 high1 = -tmp;
371 }
372
373 _LinePolyGetBounds(poly2, low2, high2);
374
375 low2 -= data.p2[0];
376 high2 -= data.p2[0];
377
378 if(data.v2[0] < 0) { // v2 = (-1, 0)
379 CoordType tmp = low2;
380 low2 = -high2;
381 high2 = -tmp;
382 }
383
384 return high1 >= low2 && high2 >= low1;
385 }
386
387 if(data.o1_is_line) {
388 assert(!proper);
389 CoordType min, max;
390 _LinePolyGetBounds(poly1, min, max);
391
392 min -= data.p1[0];
393 max -= data.p1[0];
394
395 if(data.v1[0] < 0) { // v1 = (-1, 0)
396 CoordType tmp = min;
397 min = -max;
398 max = -tmp;
399 }
400
401 Segment<2> s(data.p2 + min * data.v2, data.p1 + max * data.v2);
402
403 return Intersect(poly2, s, false);
404 }
405
406 if(data.o2_is_line) {
407 assert(!proper);
408 CoordType min, max;
409 _LinePolyGetBounds(poly2, min, max);
410
411 min -= data.p2[0];
412 max -= data.p2[0];
413
414 if(data.v2[0] < 0) { // v2 = (-1, 0)
415 CoordType tmp = min;
416 min = -max;
417 max = -tmp;
418 }
419
420 Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
421
422 return Intersect(poly1, s, false);
423 }
424
425 {
426 std::vector<CoordType> cross1(poly1.numCorners());
427 if(!_GetCrossings(poly1, data.p1, data.v1, cross1, proper))
428 return false; // line misses polygon
429
430 std::vector<CoordType> cross2(poly2.numCorners());
431 if(!_GetCrossings(poly2, data.p2, data.v2, cross2, proper))
432 return false; // line misses polygon
433
434 std::vector<CoordType>::iterator i1 = cross1.begin(), i2 = cross2.begin();
435 bool hit1 = false, hit2 = false;
436
437 while(i1 != cross1.end() && i2 != cross2.end()) {
438 if(Equal(*i1, *i2)) {
439 if(!proper)
440 return true;
441
442 hit1 = !hit1;
443 ++i1;
444 hit2 = !hit2;
445 ++i2;
446 }
447
448 if(*i1 < *i2) {
449 hit1 = !hit1;
450 ++i1;
451 }
452 else {
453 hit2 = !hit2;
454 ++i2;
455 }
456
457 if(hit1 && hit2)
458 return true;
459 }
460
461 return false;
462 }
463 case 2:
464 // Shift one polygon into the other's coordinates.
465 // Perhaps not the most efficient, but this is a
466 // rare special case.
467 {
468 Polygon<2> tmp_poly(poly2);
469
470 for(size_t i = 0; i < tmp_poly.numCorners(); ++i) {
471 Point<2> &p = tmp_poly[i];
472 Point<2> shift_p = p + data.off;
473
474 p[0] = shift_p[0] * data.v1[0] + shift_p[1] * data.v2[0];
475 p[1] = shift_p[0] * data.v1[1] + shift_p[1] * data.v2[1];
476 }
477
478 return Intersect(poly1, tmp_poly, proper);
479 }
480 default:
481 assert(false);
482 return false;
483 }
484 }
485
_PolyPolyContains(const Polygon<2> & outer,const Polygon<2> & inner,const int intersect_dim,const _Poly2OrientIntersectData & data,bool proper)486 bool _PolyPolyContains(const Polygon<2> &outer, const Polygon<2> &inner,
487 const int intersect_dim,
488 const _Poly2OrientIntersectData &data, bool proper)
489 {
490 switch(intersect_dim) {
491 case -1:
492 return false;
493 case 0:
494 return Contains(data.p2, inner, false)
495 && Contains(outer, data.p1, proper);
496 case 1:
497 if(!data.o2_is_line) // The inner poly isn't contained by the intersect line
498 return false;
499
500 // The inner poly lies on a line, so it reduces to a line segment
501 {
502 CoordType min, max;
503 _LinePolyGetBounds(inner, min, max);
504
505 min -= data.p2[0];
506 max -= data.p2[0];
507
508 if(data.v2[0] < 0) { // v2 = (-1, 0)
509 CoordType tmp = min;
510 min = -max;
511 max = -tmp;
512 }
513
514 Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
515
516 return Contains(outer, s, proper);
517 }
518 case 2:
519 // Shift one polygon into the other's coordinates.
520 // Perhaps not the most efficient, but this is a
521 // rare special case.
522 {
523 Polygon<2> tmp_poly(inner);
524
525 for(size_t i = 0; i < tmp_poly.numCorners(); ++i) {
526 Point<2> &p = tmp_poly[i];
527 Point<2> shift_p = p + data.off;
528
529 p[0] = shift_p[0] * data.v1[0] + shift_p[1] * data.v2[0];
530 p[1] = shift_p[0] * data.v1[1] + shift_p[1] * data.v2[1];
531 }
532
533 return Contains(outer, tmp_poly, proper);
534 }
535 default:
536 assert(false);
537 return false;
538 }
539 }
540
541 // Polygon<2> intersection functions
542
543 // FIXME deal with round off error in _all_ these intersection functions
544
545 // The Polygon<2>/Point<2> intersection function was stolen directly
546 // from shape.cpp in libCoal
547
548 template<>
Intersect(const Polygon<2> & r,const Point<2> & p,bool proper)549 bool Intersect<2>(const Polygon<2>& r, const Point<2>& p, bool proper)
550 {
551 const Polygon<2>::theConstIter begin = r.m_points.begin(), end = r.m_points.end();
552 bool hit = false;
553
554 for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
555 bool vertically_between =
556 (((*i)[1] <= p[1] && p[1] < (*j)[1]) ||
557 ((*j)[1] <= p[1] && p[1] < (*i)[1]));
558
559 if (!vertically_between)
560 continue;
561
562 CoordType x_intersect = (*i)[0] + ((*j)[0] - (*i)[0])
563 * (p[1] - (*i)[1]) / ((*j)[1] - (*i)[1]);
564
565 if(Equal(p[0], x_intersect))
566 return !proper;
567
568 if(p[0] < x_intersect)
569 hit = !hit;
570 }
571
572 return hit;
573 }
574
575 template<>
Contains(const Point<2> & p,const Polygon<2> & r,bool proper)576 bool Contains<2>(const Point<2>& p, const Polygon<2>& r, bool proper)
577 {
578 if(proper) // Weird degenerate case
579 return r.numCorners() == 0;
580
581 for(unsigned int i = 0; i < r.m_points.size(); ++i)
582 if(p != r.m_points[i])
583 return false;
584
585 return true;
586 }
587
588 template<>
Intersect(const Polygon<2> & p,const AxisBox<2> & b,bool proper)589 bool Intersect<2>(const Polygon<2>& p, const AxisBox<2>& b, bool proper)
590 {
591 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
592 bool hit = false;
593
594 for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
595 bool low_vertically_between =
596 (((*i)[1] <= b.m_low[1] && b.m_low[1] < (*j)[1]) ||
597 ((*j)[1] <= b.m_low[1] && b.m_low[1] < (*i)[1]));
598 bool low_horizontally_between =
599 (((*i)[0] <= b.m_low[0] && b.m_low[0] < (*j)[0]) ||
600 ((*j)[0] <= b.m_low[0] && b.m_low[0] < (*i)[0]));
601 bool high_vertically_between =
602 (((*i)[1] <= b.m_high[1] && b.m_high[1] < (*j)[1]) ||
603 ((*j)[1] <= b.m_high[1] && b.m_high[1] < (*i)[1]));
604 bool high_horizontally_between =
605 (((*i)[0] <= b.m_high[0] && b.m_high[0] < (*j)[0]) ||
606 ((*j)[0] <= b.m_high[0] && b.m_high[0] < (*i)[0]));
607
608 CoordType xdiff = ((*j)[0] - (*i)[0]);
609 CoordType ydiff = ((*j)[1] - (*i)[1]);
610
611 if(low_vertically_between) { // Check for edge intersect
612 CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
613 * xdiff / ydiff;
614
615 if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
616 return !proper;
617 if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
618 return true;
619
620 // Also check for point inclusion here, only need to do this for one point
621 if(b.m_low[0] < x_intersect)
622 hit = !hit;
623 }
624
625 if(low_horizontally_between) { // Check for edge intersect
626 CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
627 * ydiff / xdiff;
628
629 if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
630 return !proper;
631 if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
632 return true;
633 }
634
635 if(high_vertically_between) { // Check for edge intersect
636 CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
637 * xdiff / ydiff;
638
639 if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
640 return !proper;
641 if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
642 return true;
643 }
644
645 if(high_horizontally_between) { // Check for edge intersect
646 CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
647 * ydiff / xdiff;
648
649 if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
650 return !proper;
651 if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
652 return true;
653 }
654 }
655
656 return hit;
657 }
658
659 template<>
Contains(const Polygon<2> & p,const AxisBox<2> & b,bool proper)660 bool Contains<2>(const Polygon<2>& p, const AxisBox<2>& b, bool proper)
661 {
662 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
663 bool hit = false;
664
665 for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
666 bool low_vertically_between =
667 (((*i)[1] <= b.m_low[1] && b.m_low[1] < (*j)[1]) ||
668 ((*j)[1] <= b.m_low[1] && b.m_low[1] < (*i)[1]));
669 bool low_horizontally_between =
670 (((*i)[0] <= b.m_low[0] && b.m_low[0] < (*j)[0]) ||
671 ((*j)[0] <= b.m_low[0] && b.m_low[0] < (*i)[0]));
672 bool high_vertically_between =
673 (((*i)[1] <= b.m_high[1] && b.m_high[1] < (*j)[1]) ||
674 ((*j)[1] <= b.m_high[1] && b.m_high[1] < (*i)[1]));
675 bool high_horizontally_between =
676 (((*i)[0] <= b.m_high[0] && b.m_high[0] < (*j)[0]) ||
677 ((*j)[0] <= b.m_high[0] && b.m_high[0] < (*i)[0]));
678
679 CoordType xdiff = ((*j)[0] - (*i)[0]);
680 CoordType ydiff = ((*j)[1] - (*i)[1]);
681
682 if(low_vertically_between) { // Check for edge intersect
683 CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
684 * xdiff / ydiff;
685
686 bool on_corner = Equal(b.m_low[0], x_intersect)
687 || Equal(b.m_high[0], x_intersect);
688
689 if(on_corner && proper)
690 return false;
691
692 if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
693 return false;
694
695 // Also check for point inclusion here, only need to do this for one point
696 if(!on_corner && b.m_low[0] < x_intersect)
697 hit = !hit;
698 }
699
700 if(low_horizontally_between) { // Check for edge intersect
701 CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
702 * ydiff / xdiff;
703
704 bool on_corner = Equal(b.m_low[1], y_intersect)
705 || Equal(b.m_high[1], y_intersect);
706
707 if(on_corner && proper)
708 return false;
709
710 if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
711 return false;
712 }
713
714 if(high_vertically_between) { // Check for edge intersect
715 CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
716 * xdiff / ydiff;
717
718 bool on_corner = Equal(b.m_low[0], x_intersect)
719 || Equal(b.m_high[0], x_intersect);
720
721 if(on_corner && proper)
722 return false;
723
724 if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
725 return false;
726 }
727
728 if(high_horizontally_between) { // Check for edge intersect
729 CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
730 * ydiff / xdiff;
731
732 bool on_corner = Equal(b.m_low[1], y_intersect)
733 || Equal(b.m_high[1], y_intersect);
734
735 if(on_corner && proper)
736 return false;
737
738 if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
739 return false;
740 }
741 }
742
743 return hit;
744 }
745
746 template<>
Contains(const AxisBox<2> & b,const Polygon<2> & p,bool proper)747 bool Contains<2>(const AxisBox<2>& b, const Polygon<2>& p, bool proper)
748 {
749 for(Polygon<2>::theConstIter i = p.m_points.begin(); i != p.m_points.end(); ++i)
750 if(!Contains(b, *i, proper))
751 return false;
752
753 return true;
754 }
755
756 template<>
Intersect(const Polygon<2> & p,const Ball<2> & b,bool proper)757 bool Intersect<2>(const Polygon<2>& p, const Ball<2>& b, bool proper)
758 {
759 if(Contains(p, b.m_center, proper))
760 return true;
761
762 Segment<2> s2;
763 s2.endpoint(0) = p.m_points.back();
764 int next_end = 1;
765
766 for(Polygon<2>::theConstIter i = p.m_points.begin(); i != p.m_points.end(); ++i) {
767 s2.endpoint(next_end) = *i;
768 if(Intersect(s2, b, proper))
769 return true;
770 next_end = next_end ? 0 : 1;
771 }
772
773 return false;
774 }
775
776 template<>
Contains(const Polygon<2> & p,const Ball<2> & b,bool proper)777 bool Contains<2>(const Polygon<2>& p, const Ball<2>& b, bool proper)
778 {
779 if(!Contains(p, b.m_center, proper))
780 return false;
781
782 Segment<2> s2;
783 s2.endpoint(0) = p.m_points.back();
784 int next_end = 1;
785
786 for(Polygon<2>::theConstIter i = p.m_points.begin(); i != p.m_points.end(); ++i) {
787 s2.endpoint(next_end) = *i;
788 if(Intersect(s2, b, !proper))
789 return false;
790 next_end = next_end ? 0 : 1;
791 }
792
793 return true;
794 }
795
796 template<>
Contains(const Ball<2> & b,const Polygon<2> & p,bool proper)797 bool Contains<2>(const Ball<2>& b, const Polygon<2>& p, bool proper)
798 {
799 CoordType sqr_dist = b.m_radius * b.m_radius;
800
801 for(Polygon<2>::theConstIter i = p.m_points.begin(); i != p.m_points.end(); ++i)
802 if(_Greater(SquaredDistance(b.m_center, *i), sqr_dist, proper))
803 return false;
804
805 return true;
806 }
807
808 template<>
Intersect(const Polygon<2> & p,const Segment<2> & s,bool proper)809 bool Intersect<2>(const Polygon<2>& p, const Segment<2>& s, bool proper)
810 {
811 if(Contains(p, s.endpoint(0), proper))
812 return true;
813
814 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
815
816 Segment<2> s2;
817
818 s2.endpoint(0) = p.m_points.back();
819 int next_point = 1;
820
821 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
822 s2.endpoint(next_point) = *i;
823 if(Intersect(s, s2, proper))
824 return true;
825 next_point = next_point ? 0 : 1;
826 }
827
828 return false;
829 }
830
831 template<>
Contains(const Polygon<2> & p,const Segment<2> & s,bool proper)832 bool Contains<2>(const Polygon<2>& p, const Segment<2>& s, bool proper)
833 {
834 if(proper && !Contains(p, s.endpoint(0), true))
835 return false;
836
837 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
838
839 Segment<2> s2;
840
841 s2.endpoint(0) = p.m_points.back();
842 int next_point = 1;
843 bool hit = false;
844
845 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
846 s2.endpoint(next_point) = *i;
847 if(Intersect(s2, s, !proper))
848 return false;
849 bool this_point = next_point;
850 next_point = next_point ? 0 : 1;
851 if(proper)
852 continue;
853
854 // Check for crossing at an endpoint
855 if(Contains(s, *i, false) && (*i != s.m_p2)) {
856 Vector<2> segment = s.m_p2 - s.m_p1;
857 Vector<2> edge1 = *i - s2.endpoint(next_point); // Gives prev point in this case
858 Vector<2> edge2 = *i - *(i + 1);
859
860 CoordType c1 = Cross(segment, edge1), c2 = Cross(segment, edge2);
861
862 if(c1 * c2 < 0) { // opposite sides
863 if(*i == s.m_p1) { // really a containment issue
864 if(edge1[1] * edge2[1] > 0 // Edges either both up or both down
865 || ((edge1[1] > 0) ? c1 : c2) < 0) // segment lies to the left
866 hit = !hit;
867 continue; // Already checked containment for this point
868 }
869 else
870 return false;
871 }
872 }
873
874 // Check containment of one endpoint
875
876 // next_point also gives prev_point
877 bool vertically_between =
878 ((s2.endpoint(this_point)[1] <= s.m_p1[1]
879 && s.m_p1[1] < s2.endpoint(next_point)[1]) ||
880 (s2.endpoint(next_point)[1] <= s.m_p1[1]
881 && s.m_p1[1] < s2.endpoint(this_point)[1]));
882
883 if (!vertically_between)
884 continue;
885
886 CoordType x_intersect = s2.m_p1[0] + (s2.m_p2[0] - s2.m_p1[0])
887 * (s.m_p1[1] - s2.m_p1[1])
888 / (s2.m_p2[1] - s2.m_p1[1]);
889
890 if(Equal(s.m_p1[0], x_intersect)) { // Figure out which side the segment's on
891
892 // Equal points are handled in the crossing routine above
893 if(s2.endpoint(next_point) == s.m_p1)
894 continue;
895 assert(s2.endpoint(this_point) != s.m_p1);
896
897 Vector<2> poly_edge = (s2.m_p1[1] < s2.m_p2[1]) ? (s2.m_p2 - s2.m_p1)
898 : (s2.m_p1 - s2.m_p2);
899 Vector<2> segment = s.m_p2 - s.m_p1;
900
901 if(Cross(segment, poly_edge) < 0)
902 hit = !hit;
903 }
904 else if(s.m_p1[0] < x_intersect)
905 hit = !hit;
906 }
907
908 return proper || hit;
909 }
910
911 template<>
Contains(const Segment<2> & s,const Polygon<2> & p,bool proper)912 bool Contains<2>(const Segment<2>& s, const Polygon<2>& p, bool proper)
913 {
914 for(Polygon<2>::theConstIter i = p.m_points.begin(); i != p.m_points.end(); ++i)
915 if(!Contains(s, *i, proper))
916 return false;
917
918 return true;
919 }
920
921 template<>
Intersect(const Polygon<2> & p,const RotBox<2> & r,bool proper)922 bool Intersect<2>(const Polygon<2>& p, const RotBox<2>& r, bool proper)
923 {
924 CoordType m_low[2], m_high[2];
925
926 for(int j = 0; j < 2; ++j) {
927 if(r.m_size[j] > 0) {
928 m_low[j] = r.m_corner0[j];
929 m_high[j] = r.m_corner0[j] + r.m_size[j];
930 }
931 else {
932 m_high[j] = r.m_corner0[j];
933 m_low[j] = r.m_corner0[j] + r.m_size[j];
934 }
935 }
936
937 Point<2> ends[2];
938 ends[0] = p.m_points.back();
939 // FIXME Point<>::rotateInverse()
940 ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
941 int next_end = 1;
942
943 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
944 bool hit = false;
945
946 for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
947 ends[next_end] = *i;
948 // FIXME Point<>::rotateInverse()
949 ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
950 next_end = next_end ? 0 : 1;
951
952 bool low_vertically_between =
953 (((ends[0])[1] <= m_low[1] && m_low[1] < (ends[1])[1]) ||
954 ((ends[1])[1] <= m_low[1] && m_low[1] < (ends[0])[1]));
955 bool low_horizontally_between =
956 (((ends[0])[0] <= m_low[0] && m_low[0] < (ends[1])[0]) ||
957 ((ends[1])[0] <= m_low[0] && m_low[0] < (ends[0])[0]));
958 bool high_vertically_between =
959 (((ends[0])[1] <= m_high[1] && m_high[1] < (ends[1])[1]) ||
960 ((ends[1])[1] <= m_high[1] && m_high[1] < (ends[0])[1]));
961 bool high_horizontally_between =
962 (((ends[0])[0] <= m_high[0] && m_high[0] < (ends[1])[0]) ||
963 ((ends[1])[0] <= m_high[0] && m_high[0] < (ends[0])[0]));
964
965 CoordType xdiff = (ends[1])[0] - (ends[0])[0];
966 CoordType ydiff = (ends[1])[1] - (ends[0])[1];
967
968 if(low_vertically_between) { // Check for edge intersect
969 CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
970 * xdiff / ydiff;
971
972 if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
973 return !proper;
974 if(m_low[0] < x_intersect && m_high[0] > x_intersect)
975 return true;
976
977 // Also check for point inclusion here, only need to do this for one point
978 if(m_low[0] < x_intersect)
979 hit = !hit;
980 }
981
982 if(low_horizontally_between) { // Check for edge intersect
983 CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
984 * ydiff / xdiff;
985
986 if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
987 return !proper;
988 if(m_low[1] < y_intersect && m_high[1] > y_intersect)
989 return true;
990 }
991
992 if(high_vertically_between) { // Check for edge intersect
993 CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
994 * xdiff / ydiff;
995
996 if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
997 return !proper;
998 if(m_low[0] < x_intersect && m_high[0] > x_intersect)
999 return true;
1000 }
1001
1002 if(high_horizontally_between) { // Check for edge intersect
1003 CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1004 * ydiff / xdiff;
1005
1006 if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1007 return !proper;
1008 if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1009 return true;
1010 }
1011 }
1012
1013 return hit;
1014 }
1015
1016 template<>
Contains(const Polygon<2> & p,const RotBox<2> & r,bool proper)1017 bool Contains<2>(const Polygon<2>& p, const RotBox<2>& r, bool proper)
1018 {
1019 CoordType m_low[2], m_high[2];
1020
1021 for(int j = 0; j < 2; ++j) {
1022 if(r.m_size[j] > 0) {
1023 m_low[j] = r.m_corner0[j];
1024 m_high[j] = r.m_corner0[j] + r.m_size[j];
1025 }
1026 else {
1027 m_high[j] = r.m_corner0[j];
1028 m_low[j] = r.m_corner0[j] + r.m_size[j];
1029 }
1030 }
1031
1032 Point<2> ends[2];
1033 ends[0] = p.m_points.back();
1034 // FIXME Point<>::rotateInverse()
1035 ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1036 int next_end = 1;
1037
1038 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1039 bool hit = false;
1040
1041 for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1042 ends[next_end] = *i;
1043 // FIXME Point<>::rotateInverse()
1044 ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1045 next_end = next_end ? 0 : 1;
1046
1047 bool low_vertically_between =
1048 (((ends[0])[1] <= m_low[1] && m_low[1] < (ends[1])[1]) ||
1049 ((ends[1])[1] <= m_low[1] && m_low[1] < (ends[0])[1]));
1050 bool low_horizontally_between =
1051 (((ends[0])[0] <= m_low[0] && m_low[0] < (ends[1])[0]) ||
1052 ((ends[1])[0] <= m_low[0] && m_low[0] < (ends[0])[0]));
1053 bool high_vertically_between =
1054 (((ends[0])[1] <= m_high[1] && m_high[1] < (ends[1])[1]) ||
1055 ((ends[1])[1] <= m_high[1] && m_high[1] < (ends[0])[1]));
1056 bool high_horizontally_between =
1057 (((ends[0])[0] <= m_high[0] && m_high[0] < (ends[1])[0]) ||
1058 ((ends[1])[0] <= m_high[0] && m_high[0] < (ends[0])[0]));
1059
1060 CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1061 CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1062
1063 if(low_vertically_between) { // Check for edge intersect
1064 CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1065 * xdiff / ydiff;
1066
1067 bool on_corner = Equal(m_low[0], x_intersect)
1068 || Equal(m_high[0], x_intersect);
1069
1070 if(on_corner && proper)
1071 return false;
1072
1073 if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1074 return false;
1075
1076 // Also check for point inclusion here, only need to do this for one point
1077 if(!on_corner && m_low[0] < x_intersect)
1078 hit = !hit;
1079 }
1080
1081 if(low_horizontally_between) { // Check for edge intersect
1082 CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1083 * ydiff / xdiff;
1084
1085 bool on_corner = Equal(m_low[1], y_intersect)
1086 || Equal(m_high[1], y_intersect);
1087
1088 if(on_corner && proper)
1089 return false;
1090
1091 if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1092 return false;
1093 }
1094
1095 if(high_vertically_between) { // Check for edge intersect
1096 CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1097 * xdiff / ydiff;
1098
1099 bool on_corner = Equal(m_low[0], x_intersect)
1100 || Equal(m_high[0], x_intersect);
1101
1102 if(on_corner && proper)
1103 return false;
1104
1105 if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1106 return false;
1107 }
1108
1109 if(high_horizontally_between) { // Check for edge intersect
1110 CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1111 * ydiff / xdiff;
1112
1113 bool on_corner = Equal(m_low[1], y_intersect)
1114 || Equal(m_high[1], y_intersect);
1115
1116 if(on_corner && proper)
1117 return false;
1118
1119 if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1120 return false;
1121 }
1122 }
1123
1124 return hit;
1125 }
1126
1127 template<>
Contains(const RotBox<2> & r,const Polygon<2> & p,bool proper)1128 bool Contains<2>(const RotBox<2>& r, const Polygon<2>& p, bool proper)
1129 {
1130 for(Polygon<2>::theConstIter i = p.m_points.begin(); i != p.m_points.end(); ++i)
1131 if(!Contains(r, *i, proper))
1132 return false;
1133
1134 return true;
1135 }
1136
1137 template<>
Intersect(const Polygon<2> & p1,const Polygon<2> & p2,bool proper)1138 bool Intersect<2>(const Polygon<2>& p1, const Polygon<2>& p2, bool proper)
1139 {
1140 Polygon<2>::theConstIter begin1 = p1.m_points.begin(), end1 = p1.m_points.end();
1141 Polygon<2>::theConstIter begin2 = p2.m_points.begin(), end2 = p2.m_points.end();
1142 Segment<2> s1, s2;
1143 int next_end1, next_end2;
1144
1145 s1.endpoint(0) = p1.m_points.back();
1146 s2.endpoint(0) = p2.m_points.back();
1147 next_end1 = next_end2 = 1;
1148 for(Polygon<2>::theConstIter i1 = begin1; i1 != end1; ++i1) {
1149 s1.endpoint(next_end1) = *i1;
1150 next_end1 = next_end1 ? 0 : 1;
1151
1152 for(Polygon<2>::theConstIter i2 = begin2; i2 != end2; ++i2) {
1153 s2.endpoint(next_end2) = *i2;
1154 next_end2 = next_end2 ? 0 : 1;
1155
1156 if(Intersect(s1, s2, proper))
1157 return true;
1158 }
1159 }
1160
1161 return Contains(p1, p2.m_points.front(), proper)
1162 || Contains(p2, p1.m_points.front(), proper);
1163 }
1164
1165 template<>
Contains(const Polygon<2> & outer,const Polygon<2> & inner,bool proper)1166 bool Contains<2>(const Polygon<2>& outer, const Polygon<2>& inner, bool proper)
1167 {
1168 if(proper && !Contains(outer, inner.m_points.front(), true))
1169 return false;
1170
1171 Polygon<2>::theConstIter begin = inner.m_points.begin(), end = inner.m_points.end();
1172 Segment<2> s;
1173 s.endpoint(0) = inner.m_points.back();
1174 int next_end = 1;
1175
1176 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1177 s.endpoint(next_end) = *i;
1178 next_end = next_end ? 0 : 1;
1179 if(!proper) {
1180 if(!Contains(outer, s, false))
1181 return false;
1182 }
1183 else {
1184 Polygon<2>::theConstIter begin2 = outer.m_points.begin(),
1185 end2 = outer.m_points.end();
1186 Segment<2> s2;
1187 s2.endpoint(0) = outer.m_points.back();
1188 int next_end2 = 1;
1189 for(Polygon<2>::theConstIter i2 = begin2; i2 != end2; ++i2) {
1190 s2.endpoint(next_end2) = *i2;
1191 next_end2 = next_end2 ? 0 : 1;
1192
1193 if(Intersect(s, s2, false))
1194 return false;
1195 }
1196 }
1197 }
1198
1199 return true;
1200 }
1201
1202 }
1203