26 #include "polygon_intersect.h"
40 inline Vector<dim> Poly2Orient<dim>::offset(
const Point<dim>& pd, Point<2>& p2)
const
42 assert(m_origin.isValid());
44 Vector<dim> out = pd - m_origin;
46 for(
int j = 0; j < 2; ++j) {
47 p2[j] = Dot(out, m_axes[j]);
48 out -= p2[j] * m_axes[j];
55 inline bool Poly2Orient<dim>::checkContained(
const Point<dim>& pd, Point<2> & p2)
const
57 Vector<dim> off = offset(pd, p2);
60 for(
int i = 0; i < dim; ++i)
61 sqrsum += pd[i] * pd[i];
67 bool Poly2Orient<3>::checkIntersectPlane(
const AxisBox<3>& b, Point<2>& p2,
71 bool Poly2Orient<dim>::checkIntersect(
const AxisBox<dim>& b, Point<2>& p2,
74 assert(m_origin.isValid());
76 if(!m_axes[0].isValid()) {
79 return Intersect(b, convert(p2), proper);
82 if(m_axes[1].isValid()) {
88 return checkIntersectPlane(b, p2, proper);
96 bool got_bounds =
false;
98 for(
int i = 0; i < dim; ++i) {
101 if(_Less(m_origin[i], b.lowCorner()[i], proper)
102 || _Greater(m_origin[i], b.highCorner()[i], proper))
106 CoordType low = (b.lowCorner()[i] - m_origin[i]) / dist;
107 CoordType high = (b.highCorner()[i] - m_origin[i]) / dist;
129 if(_LessEq(min, max, proper)) {
130 p2[0] = (max - min) / 2;
139 int Intersect(
const Poly2Orient<dim> &o1,
const Poly2Orient<dim> &o2,
140 Poly2OrientIntersectData &data)
142 if(!o1.m_origin.isValid() || !o2.m_origin.isValid()) {
148 if(!o1.m_axes[0].isValid()) {
149 if(!o2.checkContained(o1.m_origin, data.p2))
154 data.p1[0] = data.p1[1] = 0;
159 if(!o2.m_axes[0].isValid()) {
160 if(!o1.checkContained(o2.m_origin, data.p1))
163 data.p2[0] = data.p2[1] = 0;
172 Vector<dim> basis1, basis2;
176 basis1 = o2.m_axes[0] * Dot(o2.m_axes[0], o1.m_axes[0]);
177 if(o2.m_axes[1].isValid())
178 basis1 += o2.m_axes[1] * Dot(o2.m_axes[1], o1.m_axes[0]);
181 sqrmag1 = basis1.sqrMag();
185 if(o1.m_axes[1].isValid()) {
186 basis2 = o2.m_axes[0] * Dot(o2.m_axes[0], o1.m_axes[1]);
187 if(o2.m_axes[1].isValid())
188 basis2 += o2.m_axes[1] * Dot(o2.m_axes[1], o1.m_axes[1]);
192 basis2 -= basis1 * (Dot(basis1, basis2) / sqrmag1);
194 sqrmag2 = basis2.sqrMag();
196 if(basis_size++ == 0) {
203 Vector<dim> off = o2.m_origin - o1.m_origin;
211 data.p1[0] = Dot(o1.m_axes[0], off);
212 Vector<dim> off1 = o1.m_axes[0] * data.p1[0];
213 if(o1.m_axes[1].isValid()) {
214 data.p1[1] = Dot(o1.m_axes[1], off);
215 off1 += o1.m_axes[1] * data.p1[1];
220 data.p2[0] = -Dot(o2.m_axes[0], off);
221 Vector<dim> off2 = o2.m_axes[0] * data.p2[0];
222 if(o1.m_axes[1].isValid()) {
223 data.p2[1] = -Dot(o2.m_axes[1], off);
224 off2 += o1.m_axes[1] * data.p2[1];
229 if(off1 - off2 != off)
238 data.o1_is_line = !o1.m_axes[1].isValid();
239 data.o2_is_line = !o2.m_axes[1].isValid();
241 if(!o1.m_axes[1].isValid() && !o2.m_axes[1].isValid()) {
243 if(off != o2.m_axes[0] * proj)
248 data.p1[0] = data.p1[1] = 0;
249 data.v2[0] = (Dot(o1.m_axes[0], o2.m_axes[0]) > 0) ? 1 : -1;
257 if(!o1.m_axes[1].isValid()) {
258 data.p2[0] = -Dot(off, o2.m_axes[0]);
259 data.p2[1] = -Dot(off, o2.m_axes[1]);
261 if(off != - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
266 data.p1[0] = data.p1[1] = 0;
267 data.v2[0] = Dot(o1.m_axes[0], o2.m_axes[0]);
268 data.v2[1] = Dot(o1.m_axes[0], o2.m_axes[1]);
273 if(!o2.m_axes[1].isValid()) {
274 data.p1[0] = Dot(off, o1.m_axes[0]);
275 data.p1[1] = Dot(off, o1.m_axes[1]);
277 if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1])
282 data.p2[0] = data.p2[1] = 0;
283 data.v1[0] = Dot(o1.m_axes[0], o2.m_axes[0]);
284 data.v1[1] = Dot(o1.m_axes[1], o2.m_axes[0]);
289 data.p1[0] = Dot(off, o1.m_axes[0]);
290 data.p1[1] = Dot(off, o1.m_axes[1]);
291 data.p2[0] = -Dot(off, o2.m_axes[0]);
292 data.p2[1] = -Dot(off, o2.m_axes[1]);
294 if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1]
295 - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
298 basis1 /= std::sqrt(sqrmag1);
300 data.v1[0] = Dot(o1.m_axes[0], basis1);
301 data.v1[1] = Dot(o1.m_axes[1], basis1);
302 data.v2[0] = Dot(o2.m_axes[0], basis1);
303 data.v2[1] = Dot(o2.m_axes[1], basis1);
309 assert(o1.m_axes[1].isValid() && o2.m_axes[1].isValid());
312 CoordType off_sqr_mag = data.off.sqrMag();
316 if(off_sqr_mag != 0) {
317 Vector<dim> off_copy = off;
319 data.off[0] = Dot(o2.m_axes[0], off);
320 off_copy -= o1.m_axes[0] * data.off[0];
321 data.off[1] = Dot(o2.m_axes[1], off);
322 off_copy -= o1.m_axes[1] * data.off[1];
328 data.off[0] = data.off[1] = 0;
331 data.v1[0] = Dot(o2.m_axes[0], o1.m_axes[0]);
332 data.v1[1] = Dot(o2.m_axes[0], o1.m_axes[1]);
333 data.v2[0] = Dot(o2.m_axes[1], o1.m_axes[0]);
334 data.v2[1] = Dot(o2.m_axes[1], o1.m_axes[1]);
345 inline bool Intersect(
const Polygon<dim>& r,
const Point<dim>& p,
bool proper)
349 return r.m_poly.numCorners() > 0 && r.m_orient.checkContained(p, p2)
350 && Intersect(r.m_poly, p2, proper);
354 inline bool Contains(
const Point<dim>& p,
const Polygon<dim>& r,
bool proper)
356 if(r.m_poly.numCorners() == 0)
362 for(
size_t i = 1; i < r.m_poly.numCorners(); ++i)
363 if(r.m_poly[i] != r.m_poly[0])
368 return r.m_orient.checkContained(p, p2) && p2 == r.m_poly[0];
372 bool Intersect(
const Polygon<dim>& p,
const AxisBox<dim>& b,
bool proper)
374 size_t corners = p.m_poly.numCorners();
381 if(!p.m_orient.checkIntersect(b, p2, proper))
385 s.endpoint(0) = p.m_orient.convert(p.m_poly.getCorner(corners-1));
388 for(
size_t i = 0; i < corners; ++i) {
389 s.endpoint(next_end) = p.m_orient.convert(p.m_poly.getCorner(i));
390 if(Intersect(b, s, proper))
392 next_end = next_end ? 0 : 1;
395 return Contains(p, p2, proper);
399 bool _PolyContainsBox(
const Poly2Orient<dim> &orient,
const Polygon<2> &poly,
400 const Point<dim> &corner,
const Vector<dim> &size,
bool proper)
402 int num_dim = 0, nonzero_dim = -1;
404 for(
int i = 0; i < dim; ++i) {
409 if(nonzero_dim == -1 || std::fabs(size[nonzero_dim]) < std::fabs(size[i]))
416 if(!orient.checkContained(corner, corner1))
420 return Contains(poly, corner1, proper);
424 if(!orient.checkContained(corner + size, corner2))
428 return Contains(poly, Segment<2>(corner1, corner2), proper);
430 Point<dim> other_corner = corner;
431 other_corner[nonzero_dim] += size[nonzero_dim];
434 if(!orient.checkContained(other_corner, corner3))
439 Vector<2> vec1(corner2 - corner1), vec2(corner3 - corner1);
444 m.rotation(Vector<2>(1, 0), vec1);
446 catch(
const ColinearVectors<2>&) {
450 RotBox<2> box(corner1,
ProdInv(vec2, m), m);
452 return Contains(poly, box, proper);
456 inline bool Contains(
const Polygon<dim>& p,
const AxisBox<dim>& b,
bool proper)
458 return _PolyContainsBox(p.m_orient, p.m_poly, b.m_low, b.m_high - b.m_low, proper);
462 inline bool Contains(
const AxisBox<dim>& b,
const Polygon<dim>& p,
bool proper)
464 for(
size_t i = 0; i < p.m_poly.numCorners(); ++i)
465 if(!Contains(b, p.getCorner(i), proper))
472 inline bool Intersect(
const Polygon<dim>& p,
const Ball<dim>& b,
bool proper)
474 if(p.m_poly.numCorners() == 0)
480 dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
482 if(_Less(dist, 0, proper))
485 return Intersect(p.m_poly, Ball<2>(c2, std::sqrt(dist)), proper);
489 inline bool Contains(
const Polygon<dim>& p,
const Ball<dim>& b,
bool proper)
491 if(p.m_poly.numCorners() == 0)
499 if(!p.m_orient.checkContained(b.m_center, c2))
502 return Contains(p.m_poly, c2, proper);
506 inline bool Contains(
const Ball<dim>& b,
const Polygon<dim>& p,
bool proper)
508 if(p.m_poly.numCorners() == 0)
514 dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
516 if(_Less(dist, 0, proper))
519 for(
size_t i = 0; i != p.m_poly.numCorners(); ++i)
520 if(_Less(dist, SquaredDistance(c2, p.m_poly[i]), proper))
527 bool Intersect(
const Polygon<dim>& p,
const Segment<dim>& s,
bool proper)
529 if(p.m_poly.numCorners() == 0)
536 v1 = p.m_orient.offset(s.m_p1, p1);
537 v2 = p.m_orient.offset(s.m_p2, p2);
544 Point<2> p_intersect;
547 return Intersect(p.m_poly, Segment<2>(p1, p2), proper);
549 for(
int i = 0; i < 2; ++i)
550 p_intersect[i] = (p1[i] * d2 + p2[i] * d1) / (d1 + d2);
552 return Intersect(p.m_poly, p_intersect, proper);
556 inline bool Contains(
const Polygon<dim>& p,
const Segment<dim>& s,
bool proper)
558 if(p.m_poly.numCorners() == 0)
563 if(!p.m_orient.checkContained(s.m_p1, s2.endpoint(0)))
565 if(!p.m_orient.checkContained(s.m_p2, s2.endpoint(1)))
568 return Contains(p.m_poly, s2, proper);
572 inline bool Contains(
const Segment<dim>& s,
const Polygon<dim>& p,
bool proper)
574 if(p.m_poly.numCorners() == 0)
581 Poly2Orient<dim> orient(p.m_orient);
583 for(
int i = 0; i < 2; ++i)
584 if(!orient.expand(s.endpoint(i), s2.endpoint(i)))
587 return Contains(s2, p.m_poly, proper);
591 bool Intersect(
const Polygon<dim>& p,
const RotBox<dim>& r,
bool proper)
593 size_t corners = p.m_poly.numCorners();
598 Poly2Orient<dim> orient(p.m_orient);
600 orient.rotate(r.m_orient.inverse(), r.m_corner0);
602 AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
606 if(!orient.checkIntersect(b, p2, proper))
610 s.endpoint(0) = orient.convert(p.m_poly.getCorner(corners-1));
613 for(
size_t i = 0; i < corners; ++i) {
614 s.endpoint(next_end) = orient.convert(p.m_poly.getCorner(i));
615 if(Intersect(b, s, proper))
617 next_end = next_end ? 0 : 1;
620 return Contains(p, p2, proper);
624 inline bool Contains(
const Polygon<dim>& p,
const RotBox<dim>& r,
bool proper)
626 Poly2Orient<dim> orient(p.m_orient);
627 orient.rotate(r.m_orient.inverse(), r.m_corner0);
629 return _PolyContainsBox(orient, p.m_poly, r.m_corner0, r.m_size, proper);
633 inline bool Contains(
const RotBox<dim>& r,
const Polygon<dim>& p,
bool proper)
635 if(p.m_poly.numCorners() == 0)
638 AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
640 Poly2Orient<dim> orient(p.m_orient);
641 orient.rotate(r.m_orient.inverse(), r.m_corner0);
643 for(
size_t i = 0; i < p.m_poly.numCorners(); ++i)
644 if(!Contains(b, orient.convert(p.m_poly[i]), proper))
650 bool PolyPolyIntersect(
const Polygon<2> &poly1,
const Polygon<2> &poly2,
652 const Poly2OrientIntersectData &data,
bool proper);
655 inline bool Intersect(
const Polygon<dim>& p1,
const Polygon<dim>& p2,
bool proper)
657 Poly2OrientIntersectData data;
659 int intersect_dim = Intersect(p1.m_orient, p2.m_orient, data);
661 return PolyPolyIntersect(p1.m_poly, p2.m_poly, intersect_dim, data, proper);
664 bool PolyPolyContains(
const Polygon<2> &outer,
const Polygon<2> &inner,
666 const Poly2OrientIntersectData &data,
bool proper);
669 inline bool Contains(
const Polygon<dim>& outer,
const Polygon<dim>& inner,
bool proper)
671 if(outer.m_poly.numCorners() == 0)
672 return !proper && inner.m_poly.numCorners() == 0;
674 if(inner.m_poly.numCorners() == 0)
677 Poly2OrientIntersectData data;
679 int intersect_dim = Intersect(outer.m_orient, inner.m_orient, data);
681 return PolyPolyContains(outer.m_poly, inner.m_poly, intersect_dim, data, proper);
687 template bool Intersect<Point<2>,Polygon<2> >(
const Point<2>&,
const Polygon<2>&, bool);
688 template bool Intersect<Point<3>,Polygon<3> >(
const Point<3>&,
const Polygon<3>&, bool);
689 template bool Contains<3>(
const Point<3>&,
const Polygon<3>&,
bool);
690 template bool Intersect<3>(
const Polygon<3>&,
const Point<3>&,
bool);
691 template bool Contains<3>(
const Polygon<3>&,
const Point<3>&,
bool);
693 template bool Intersect<AxisBox<2>,Polygon<2> >(
const AxisBox<2>&,
const Polygon<2>&, bool);
694 template bool Intersect<AxisBox<3>,Polygon<3> >(
const AxisBox<3>&,
const Polygon<3>&, bool);
695 template bool Contains<3>(
const AxisBox<3>&,
const Polygon<3>&,
bool);
696 template bool Intersect<3>(
const Polygon<3>&,
const AxisBox<3>&,
bool);
697 template bool Contains<3>(
const Polygon<3>&,
const AxisBox<3>&,
bool);
699 template bool Intersect<Ball<2>,Polygon<2> >(
const Ball<2>&,
const Polygon<2>&, bool);
700 template bool Intersect<Ball<3>,Polygon<3> >(
const Ball<3>&,
const Polygon<3>&, bool);
701 template bool Contains<3>(
const Ball<3>&,
const Polygon<3>&,
bool);
702 template bool Intersect<3>(
const Polygon<3>&,
const Ball<3>&,
bool);
703 template bool Contains<3>(
const Polygon<3>&,
const Ball<3>&,
bool);
705 template bool Intersect<Segment<2>,Polygon<2> >(
const Segment<2>&,
const Polygon<2>&, bool);
706 template bool Intersect<Segment<3>,Polygon<3> >(
const Segment<3>&,
const Polygon<3>&, bool);
707 template bool Contains<3>(
const Segment<3>&,
const Polygon<3>&,
bool);
708 template bool Intersect<3>(
const Polygon<3>&,
const Segment<3>&,
bool);
709 template bool Contains<3>(
const Polygon<3>&,
const Segment<3>&,
bool);
711 template bool Intersect<RotBox<2>,Polygon<2> >(
const RotBox<2>&,
const Polygon<2>&, bool);
712 template bool Intersect<RotBox<3>,Polygon<3> >(
const RotBox<3>&,
const Polygon<3>&, bool);
713 template bool Contains<3>(
const RotBox<3>&,
const Polygon<3>&,
bool);
714 template bool Intersect<3>(
const Polygon<3>&,
const RotBox<3>&,
bool);
715 template bool Contains<3>(
const Polygon<3>&,
const RotBox<3>&,
bool);
717 template bool Intersect<3>(
const Polygon<3>&,
const Polygon<3>&,
bool);
718 template bool Contains<3>(
const Polygon<3>&,
const Polygon<3>&,
bool);
721 bool Poly2Orient<3>::checkIntersectPlane(
const AxisBox<3>& b, Point<2>& p2,
724 assert(
"This function should only be called if the orientation represents a plane" &&
725 m_origin.isValid() && m_axes[0].isValid() && m_axes[1].isValid());
727 Vector<3> normal =
Cross(m_axes[0], m_axes[1]);
735 CoordType normal_mag = normal.sloppyMag();
736 int high_corner_num = 0;
738 for(
int i = 0; i < 3; ++i) {
739 if(std::fabs(normal[i]) < normal_mag * numeric_constants<CoordType>::epsilon()) {
741 }
else if(normal[i] > 0) {
743 high_corner_num |= (1 << i);
749 int low_corner_num = high_corner_num ^ 7;
751 Point<3> high_corner = b.getCorner(high_corner_num);
752 Point<3> low_corner = b.getCorner(low_corner_num);
756 CoordType perp_size = Dot(normal, high_corner - low_corner) / normal_mag;
757 assert(perp_size >= 0);
759 if(perp_size < normal_mag * numeric_constants<CoordType>::epsilon()) {
761 return !proper && checkContained(
Midpoint(high_corner, low_corner), p2);
764 if(_Less(Dot(high_corner - m_origin, normal), 0, proper)
765 || _Less(Dot(low_corner - m_origin, normal), 0, proper))
770 Point<2> p2_high, p2_low;
772 CoordType high_dist = offset(high_corner, p2_high).mag();
773 CoordType low_dist = offset(low_corner, p2_low).mag();
775 p2 =
Midpoint(p2_high, p2_low, high_dist / (high_dist + low_dist));
781 static void LinePolyGetBounds(
const Polygon<2> &poly, CoordType &low, CoordType &high)
783 low = high = poly[0][0];
785 for(
size_t i = 0; i < poly.numCorners(); ++i) {
804 const Vector<2> &v, std::vector<CoordType> &cross,
807 assert(poly.numCorners() == cross.size());
808 assert(Equal(v.
sqrMag(), 1));
811 Point<2> old_p = poly.getCorner(poly.numCorners() - 1);
812 bool old_below = (
Cross(v, old_p - p) < 0);
816 std::list<LinePointData> line_point_data;
818 for(
size_t i = 0; i < poly.numCorners(); ++i) {
824 if(Equal(v_i_sqr_mag, proj * proj)) {
827 CoordType proj_j, low_proj = proj, high_proj = proj;
829 for(j = i + 1; j != i; j == poly.numCorners() - 1 ? j = 0 : ++j) {
830 p_j = poly.getCorner(j);
832 proj_j = Dot(v_j, v);
834 if(!Equal(v_j.
sqrMag(), proj_j * proj_j))
837 if(proj_j < low_proj)
839 if(proj_j > high_proj)
845 bool below = (Cross(v, v_j) < 0);
847 if(below == old_below && proper) {
854 if(below != old_below) {
856 cross[next_cross++] = proj;
861 cross[next_cross++] = proj;
862 cross[next_cross++] = proj;
869 LinePointData data = {low_proj, high_proj, below != old_below};
871 std::list<LinePointData>::iterator I;
873 for(I = line_point_data.begin(); I != line_point_data.end(); ++I) {
874 if(data.low > I->high)
877 if(data.high < I->low) {
878 line_point_data.insert(I, data);
884 I->low = (I->low < data.low) ? I->low : data.low;
885 I->high = (I->high > data.high) ? I->high : data.high;
886 I->cross = (I->cross != data.cross);
892 if(J->low < I->high) {
894 I->cross = (I->cross != J->cross);
895 line_point_data.erase(J);
899 if(I == line_point_data.end())
900 line_point_data.push_back(data);
909 bool below = (
Cross(v, v_i) < 0);
911 if(below != old_below) {
913 Vector<2> dist = p - old_p;
917 CoordType denom = dist_proj * dist_proj - dist_sqr_mag;
921 CoordType line_pos = (dist_proj * Dot(v_i, dist) + dist_sqr_mag * proj) / denom;
923 cross[next_cross++] = line_pos;
929 cross.resize(next_cross);
930 std::sort(cross.begin(), cross.end());
932 if(!line_point_data.empty()) {
933 auto I = line_point_data.begin();
934 auto cross_num = cross.begin();
937 while(cross_num != cross.end() && I != line_point_data.end()) {
938 if(*cross_num < I->low) {
945 if(*cross_num > I->high) {
946 hit_between = I->cross;
949 auto high_cross_num = cross_num;
953 }
while(*high_cross_num < I->high);
955 hit_between = (((high_cross_num - cross_num) % 2) != 0) != I->cross;
957 cross_num = cross.erase(cross_num, high_cross_num);
961 cross_num = cross.insert(cross_num, proper == hit ? I->low : I->high);
966 cross_num = cross.insert(cross_num, I->low);
968 cross_num = cross.insert(cross_num, I->high);
974 while(I != line_point_data.end()) {
976 cross.push_back(proper == hit ? I->low : I->high);
980 cross.push_back(I->low);
981 cross.push_back(I->high);
989 return !cross.empty();
992 bool PolyPolyIntersect(
const Polygon<2> &poly1,
const Polygon<2> &poly2,
993 const int intersect_dim,
994 const Poly2OrientIntersectData &data,
bool proper)
996 switch(intersect_dim) {
1000 return Intersect(poly1, data.p1, proper)
1001 && Intersect(poly2, data.p2, proper);
1003 if(proper && (data.o1_is_line || data.o2_is_line))
1006 if(data.o1_is_line && data.o2_is_line) {
1010 LinePolyGetBounds(poly1, low1, high1);
1013 high1 -= data.p1[0];
1015 if(data.v1[0] < 0) {
1021 LinePolyGetBounds(poly2, low2, high2);
1024 high2 -= data.p2[0];
1026 if(data.v2[0] < 0) {
1032 return high1 >= low2 && high2 >= low1;
1035 if(data.o1_is_line) {
1038 LinePolyGetBounds(poly1, min, max);
1043 if(data.v1[0] < 0) {
1049 Segment<2> s(data.p2 + min * data.v2, data.p1 + max * data.v2);
1051 return Intersect(poly2, s,
false);
1054 if(data.o2_is_line) {
1057 LinePolyGetBounds(poly2, min, max);
1062 if(data.v2[0] < 0) {
1068 Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
1070 return Intersect(poly1, s,
false);
1074 std::vector<CoordType> cross1(poly1.numCorners());
1075 if(!GetCrossings(poly1, data.p1, data.v1, cross1, proper))
1078 std::vector<CoordType> cross2(poly2.numCorners());
1079 if(!GetCrossings(poly2, data.p2, data.v2, cross2, proper))
1082 auto i1 = cross1.begin(), i2 = cross2.begin();
1083 bool hit1 =
false, hit2 =
false;
1085 while(i1 != cross1.end() && i2 != cross2.end()) {
1086 if(Equal(*i1, *i2)) {
1116 Polygon<2> tmp_poly(poly2);
1118 for(
size_t i = 0; i < tmp_poly.numCorners(); ++i) {
1119 Point<2> &p = tmp_poly[i];
1120 Point<2> shift_p = p + data.off;
1122 p[0] = shift_p[0] * data.v1[0] + shift_p[1] * data.v2[0];
1123 p[1] = shift_p[0] * data.v1[1] + shift_p[1] * data.v2[1];
1126 return Intersect(poly1, tmp_poly, proper);
1134 bool PolyPolyContains(
const Polygon<2> &outer,
const Polygon<2> &inner,
1135 const int intersect_dim,
1136 const Poly2OrientIntersectData &data,
bool proper)
1138 switch(intersect_dim) {
1142 return Contains(data.p2, inner,
false)
1143 && Contains(outer, data.p1, proper);
1145 if(!data.o2_is_line)
1151 LinePolyGetBounds(inner, min, max);
1156 if(data.v2[0] < 0) {
1162 Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
1164 return Contains(outer, s, proper);
1171 Polygon<2> tmp_poly(inner);
1173 for(
size_t i = 0; i < tmp_poly.numCorners(); ++i) {
1174 Point<2> &p = tmp_poly[i];
1175 Point<2> shift_p = p + data.off;
1177 p[0] = shift_p[0] * data.v1[0] + shift_p[1] * data.v2[0];
1178 p[1] = shift_p[0] * data.v1[1] + shift_p[1] * data.v2[1];
1181 return Contains(outer, tmp_poly, proper);
1197 bool Intersect<2>(
const Polygon<2>& r,
const Point<2>& p,
bool proper)
1199 const Polygon<2>::theConstIter begin = r.m_points.begin(), end = r.m_points.end();
1202 for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
1203 bool vertically_between =
1204 (((*i)[1] <= p[1] && p[1] < (*j)[1]) ||
1205 ((*j)[1] <= p[1] && p[1] < (*i)[1]));
1207 if (!vertically_between)
1210 CoordType x_intersect = (*i)[0] + ((*j)[0] - (*i)[0])
1211 * (p[1] - (*i)[1]) / ((*j)[1] - (*i)[1]);
1213 if(Equal(p[0], x_intersect))
1216 if(p[0] < x_intersect)
1224 bool Contains<2>(
const Point<2>& p,
const Polygon<2>& r,
bool proper)
1227 return r.numCorners() == 0;
1229 for(
const auto & point : r.m_points)
1237 bool Intersect<2>(
const Polygon<2>& p,
const AxisBox<2>& b,
bool proper)
1239 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1242 for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
1243 bool low_vertically_between =
1244 (((*i)[1] <= b.m_low[1] && b.m_low[1] < (*j)[1]) ||
1245 ((*j)[1] <= b.m_low[1] && b.m_low[1] < (*i)[1]));
1246 bool low_horizontally_between =
1247 (((*i)[0] <= b.m_low[0] && b.m_low[0] < (*j)[0]) ||
1248 ((*j)[0] <= b.m_low[0] && b.m_low[0] < (*i)[0]));
1249 bool high_vertically_between =
1250 (((*i)[1] <= b.m_high[1] && b.m_high[1] < (*j)[1]) ||
1251 ((*j)[1] <= b.m_high[1] && b.m_high[1] < (*i)[1]));
1252 bool high_horizontally_between =
1253 (((*i)[0] <= b.m_high[0] && b.m_high[0] < (*j)[0]) ||
1254 ((*j)[0] <= b.m_high[0] && b.m_high[0] < (*i)[0]));
1259 if(low_vertically_between) {
1260 CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
1263 if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
1265 if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1269 if(b.m_low[0] < x_intersect)
1273 if(low_horizontally_between) {
1274 CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
1277 if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
1279 if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1283 if(high_vertically_between) {
1284 CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
1287 if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
1289 if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1293 if(high_horizontally_between) {
1294 CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
1297 if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
1299 if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1308 bool Contains<2>(
const Polygon<2>& p,
const AxisBox<2>& b,
bool proper)
1310 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1313 for (Polygon<2>::theConstIter i = begin, j = end - 1; i != end; j = i++) {
1314 bool low_vertically_between =
1315 (((*i)[1] <= b.m_low[1] && b.m_low[1] < (*j)[1]) ||
1316 ((*j)[1] <= b.m_low[1] && b.m_low[1] < (*i)[1]));
1317 bool low_horizontally_between =
1318 (((*i)[0] <= b.m_low[0] && b.m_low[0] < (*j)[0]) ||
1319 ((*j)[0] <= b.m_low[0] && b.m_low[0] < (*i)[0]));
1320 bool high_vertically_between =
1321 (((*i)[1] <= b.m_high[1] && b.m_high[1] < (*j)[1]) ||
1322 ((*j)[1] <= b.m_high[1] && b.m_high[1] < (*i)[1]));
1323 bool high_horizontally_between =
1324 (((*i)[0] <= b.m_high[0] && b.m_high[0] < (*j)[0]) ||
1325 ((*j)[0] <= b.m_high[0] && b.m_high[0] < (*i)[0]));
1330 if(low_vertically_between) {
1331 CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
1334 bool on_corner = Equal(b.m_low[0], x_intersect)
1335 || Equal(b.m_high[0], x_intersect);
1337 if(on_corner && proper)
1340 if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1344 if(!on_corner && b.m_low[0] < x_intersect)
1348 if(low_horizontally_between) {
1349 CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
1352 bool on_corner = Equal(b.m_low[1], y_intersect)
1353 || Equal(b.m_high[1], y_intersect);
1355 if(on_corner && proper)
1358 if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1362 if(high_vertically_between) {
1363 CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
1366 bool on_corner = Equal(b.m_low[0], x_intersect)
1367 || Equal(b.m_high[0], x_intersect);
1369 if(on_corner && proper)
1372 if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1376 if(high_horizontally_between) {
1377 CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
1380 bool on_corner = Equal(b.m_low[1], y_intersect)
1381 || Equal(b.m_high[1], y_intersect);
1383 if(on_corner && proper)
1386 if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1395 bool Contains<2>(
const AxisBox<2>& b,
const Polygon<2>& p,
bool proper)
1397 for(
const auto & point : p.m_points)
1398 if(!Contains(b, point, proper))
1405 bool Intersect<2>(
const Polygon<2>& p,
const Ball<2>& b,
bool proper)
1407 if(Contains(p, b.m_center, proper))
1411 s2.endpoint(0) = p.m_points.back();
1414 for(
const auto & point : p.m_points) {
1415 s2.endpoint(next_end) = point;
1416 if(Intersect(s2, b, proper))
1418 next_end = next_end ? 0 : 1;
1425 bool Contains<2>(
const Polygon<2>& p,
const Ball<2>& b,
bool proper)
1427 if(!Contains(p, b.m_center, proper))
1431 s2.endpoint(0) = p.m_points.back();
1434 for(
const auto & point : p.m_points) {
1435 s2.endpoint(next_end) = point;
1436 if(Intersect(s2, b, !proper))
1438 next_end = next_end ? 0 : 1;
1445 bool Contains<2>(
const Ball<2>& b,
const Polygon<2>& p,
bool proper)
1447 CoordType sqr_dist = b.m_radius * b.m_radius;
1449 for(
const auto & point : p.m_points)
1450 if(_Greater(SquaredDistance(b.m_center, point), sqr_dist, proper))
1457 bool Intersect<2>(
const Polygon<2>& p,
const Segment<2>& s,
bool proper)
1459 if(Contains(p, s.endpoint(0), proper))
1462 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1466 s2.endpoint(0) = p.m_points.back();
1469 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1470 s2.endpoint(next_point) = *i;
1471 if(Intersect(s, s2, proper))
1473 next_point = next_point ? 0 : 1;
1480 bool Contains<2>(
const Polygon<2>& p,
const Segment<2>& s,
bool proper)
1482 if(proper && !Contains(p, s.endpoint(0),
true))
1485 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1489 s2.endpoint(0) = p.m_points.back();
1493 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1494 s2.endpoint(next_point) = *i;
1495 if(Intersect(s2, s, !proper))
1497 bool this_point = next_point;
1498 next_point = next_point ? 0 : 1;
1503 if(Contains(s, *i,
false) && (*i != s.m_p2)) {
1504 Vector<2> segment = s.m_p2 - s.m_p1;
1505 Vector<2> edge1 = *i - s2.endpoint(next_point);
1506 Vector<2> edge2 = *i - *(i + 1);
1512 if(edge1[1] * edge2[1] > 0
1513 || ((edge1[1] > 0) ? c1 : c2) < 0)
1525 bool vertically_between =
1526 ((s2.endpoint(this_point)[1] <= s.m_p1[1]
1527 && s.m_p1[1] < s2.endpoint(next_point)[1]) ||
1528 (s2.endpoint(next_point)[1] <= s.m_p1[1]
1529 && s.m_p1[1] < s2.endpoint(this_point)[1]));
1531 if (!vertically_between)
1534 CoordType x_intersect = s2.m_p1[0] + (s2.m_p2[0] - s2.m_p1[0])
1535 * (s.m_p1[1] - s2.m_p1[1])
1536 / (s2.m_p2[1] - s2.m_p1[1]);
1538 if(Equal(s.m_p1[0], x_intersect)) {
1541 if(s2.endpoint(next_point) == s.m_p1)
1543 assert(s2.endpoint(this_point) != s.m_p1);
1545 Vector<2> poly_edge = (s2.m_p1[1] < s2.m_p2[1]) ? (s2.m_p2 - s2.m_p1)
1546 : (s2.m_p1 - s2.m_p2);
1547 Vector<2> segment = s.m_p2 - s.m_p1;
1549 if(
Cross(segment, poly_edge) < 0)
1552 else if(s.m_p1[0] < x_intersect)
1556 return proper || hit;
1560 bool Contains<2>(
const Segment<2>& s,
const Polygon<2>& p,
bool proper)
1562 for(
const auto & point : p.m_points)
1563 if(!Contains(s, point, proper))
1570 bool Intersect<2>(
const Polygon<2>& p,
const RotBox<2>& r,
bool proper)
1574 for(
int j = 0; j < 2; ++j) {
1575 if(r.m_size[j] > 0) {
1576 m_low[j] = r.m_corner0[j];
1577 m_high[j] = r.m_corner0[j] + r.m_size[j];
1580 m_high[j] = r.m_corner0[j];
1581 m_low[j] = r.m_corner0[j] + r.m_size[j];
1586 ends[0] = p.m_points.back();
1588 ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1591 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1594 for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1595 ends[next_end] = *i;
1597 ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1598 next_end = next_end ? 0 : 1;
1600 bool low_vertically_between =
1601 (((ends[0])[1] <= m_low[1] && m_low[1] < (ends[1])[1]) ||
1602 ((ends[1])[1] <= m_low[1] && m_low[1] < (ends[0])[1]));
1603 bool low_horizontally_between =
1604 (((ends[0])[0] <= m_low[0] && m_low[0] < (ends[1])[0]) ||
1605 ((ends[1])[0] <= m_low[0] && m_low[0] < (ends[0])[0]));
1606 bool high_vertically_between =
1607 (((ends[0])[1] <= m_high[1] && m_high[1] < (ends[1])[1]) ||
1608 ((ends[1])[1] <= m_high[1] && m_high[1] < (ends[0])[1]));
1609 bool high_horizontally_between =
1610 (((ends[0])[0] <= m_high[0] && m_high[0] < (ends[1])[0]) ||
1611 ((ends[1])[0] <= m_high[0] && m_high[0] < (ends[0])[0]));
1613 CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1614 CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1616 if(low_vertically_between) {
1617 CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1620 if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
1622 if(m_low[0] < x_intersect && m_high[0] > x_intersect)
1626 if(m_low[0] < x_intersect)
1630 if(low_horizontally_between) {
1631 CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1634 if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1636 if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1640 if(high_vertically_between) {
1641 CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1644 if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
1646 if(m_low[0] < x_intersect && m_high[0] > x_intersect)
1650 if(high_horizontally_between) {
1651 CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1654 if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1656 if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1665 bool Contains<2>(
const Polygon<2>& p,
const RotBox<2>& r,
bool proper)
1669 for(
int j = 0; j < 2; ++j) {
1670 if(r.m_size[j] > 0) {
1671 m_low[j] = r.m_corner0[j];
1672 m_high[j] = r.m_corner0[j] + r.m_size[j];
1675 m_high[j] = r.m_corner0[j];
1676 m_low[j] = r.m_corner0[j] + r.m_size[j];
1681 ends[0] = p.m_points.back();
1683 ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1686 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1689 for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1690 ends[next_end] = *i;
1692 ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1693 next_end = next_end ? 0 : 1;
1695 bool low_vertically_between =
1696 (((ends[0])[1] <= m_low[1] && m_low[1] < (ends[1])[1]) ||
1697 ((ends[1])[1] <= m_low[1] && m_low[1] < (ends[0])[1]));
1698 bool low_horizontally_between =
1699 (((ends[0])[0] <= m_low[0] && m_low[0] < (ends[1])[0]) ||
1700 ((ends[1])[0] <= m_low[0] && m_low[0] < (ends[0])[0]));
1701 bool high_vertically_between =
1702 (((ends[0])[1] <= m_high[1] && m_high[1] < (ends[1])[1]) ||
1703 ((ends[1])[1] <= m_high[1] && m_high[1] < (ends[0])[1]));
1704 bool high_horizontally_between =
1705 (((ends[0])[0] <= m_high[0] && m_high[0] < (ends[1])[0]) ||
1706 ((ends[1])[0] <= m_high[0] && m_high[0] < (ends[0])[0]));
1708 CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1709 CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1711 if(low_vertically_between) {
1712 CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1715 bool on_corner = Equal(m_low[0], x_intersect)
1716 || Equal(m_high[0], x_intersect);
1718 if(on_corner && proper)
1721 if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1725 if(!on_corner && m_low[0] < x_intersect)
1729 if(low_horizontally_between) {
1730 CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1733 bool on_corner = Equal(m_low[1], y_intersect)
1734 || Equal(m_high[1], y_intersect);
1736 if(on_corner && proper)
1739 if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1743 if(high_vertically_between) {
1744 CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1747 bool on_corner = Equal(m_low[0], x_intersect)
1748 || Equal(m_high[0], x_intersect);
1750 if(on_corner && proper)
1753 if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1757 if(high_horizontally_between) {
1758 CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1761 bool on_corner = Equal(m_low[1], y_intersect)
1762 || Equal(m_high[1], y_intersect);
1764 if(on_corner && proper)
1767 if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1776 bool Contains<2>(
const RotBox<2>& r,
const Polygon<2>& p,
bool proper)
1778 for(
const auto & point : p.m_points)
1779 if(!Contains(r, point, proper))
1786 bool Intersect<2>(
const Polygon<2>& p1,
const Polygon<2>& p2,
bool proper)
1788 auto begin1 = p1.m_points.begin(), end1 = p1.m_points.end();
1789 auto begin2 = p2.m_points.begin(), end2 = p2.m_points.end();
1791 int next_end1, next_end2;
1793 s1.endpoint(0) = p1.m_points.back();
1794 s2.endpoint(0) = p2.m_points.back();
1795 next_end1 = next_end2 = 1;
1796 for(
auto i1 = begin1; i1 != end1; ++i1) {
1797 s1.endpoint(next_end1) = *i1;
1798 next_end1 = next_end1 ? 0 : 1;
1800 for(
auto i2 = begin2; i2 != end2; ++i2) {
1801 s2.endpoint(next_end2) = *i2;
1802 next_end2 = next_end2 ? 0 : 1;
1804 if(Intersect(s1, s2, proper))
1809 return Contains(p1, p2.m_points.front(), proper)
1810 || Contains(p2, p1.m_points.front(), proper);
1814 bool Contains<2>(
const Polygon<2>& outer,
const Polygon<2>& inner,
bool proper)
1816 if(proper && !Contains(outer, inner.m_points.front(),
true))
1819 auto begin = inner.m_points.begin(), end = inner.m_points.end();
1821 s.endpoint(0) = inner.m_points.back();
1824 for(
auto i = begin; i != end; ++i) {
1825 s.endpoint(next_end) = *i;
1826 next_end = next_end ? 0 : 1;
1828 if(!Contains(outer, s,
false))
1832 auto begin2 = outer.m_points.begin(),
1833 end2 = outer.m_points.end();
1835 s2.endpoint(0) = outer.m_points.back();
1837 for(
auto i2 = begin2; i2 != end2; ++i2) {
1838 s2.endpoint(next_end2) = *i2;
1839 next_end2 = next_end2 ? 0 : 1;
1841 if(Intersect(s, s2,
false))
The 2D specialization of the Polygon<> template.
CoordType sqrMag() const
The squared magnitude of a vector.
Generic library namespace.
double CoordType
Basic floating point type.
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
Point< dim > Midpoint(const Point< dim > &p1, const Point< dim > &p2, CoordType dist=0.5)
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.