26#include "polygon_intersect.h"
39inline Vector<dim> Poly2Orient<dim>::offset(
const Point<dim>& pd, Point<2>& p2)
const
41 assert(m_origin.isValid());
43 Vector<dim> out = pd - m_origin;
45 for(
int j = 0; j < 2; ++j) {
46 p2[j] = Dot(out, m_axes[j]);
47 out -= p2[j] * m_axes[j];
54inline bool Poly2Orient<dim>::checkContained(
const Point<dim>& pd, Point<2> & p2)
const
56 Vector<dim> off = offset(pd, p2);
59 for(
int i = 0; i < dim; ++i)
60 sqrsum += pd[i] * pd[i];
66bool Poly2Orient<3>::checkIntersectPlane(
const AxisBox<3>& b, Point<2>& p2,
70bool Poly2Orient<dim>::checkIntersect(
const AxisBox<dim>& b, Point<2>& p2,
73 assert(m_origin.isValid());
75 if(!m_axes[0].isValid()) {
78 return Intersect(b, convert(p2), proper);
81 if(m_axes[1].isValid()) {
87 return checkIntersectPlane(b, p2, proper);
95 bool got_bounds =
false;
97 for(
int i = 0; i < dim; ++i) {
100 if(_Less(m_origin[i], b.lowCorner()[i], proper)
101 || _Greater(m_origin[i], b.highCorner()[i], proper))
105 CoordType low = (b.lowCorner()[i] - m_origin[i]) / dist;
106 CoordType high = (b.highCorner()[i] - m_origin[i]) / dist;
128 if(_LessEq(min, max, proper)) {
129 p2[0] = (max - min) / 2;
138int Intersect(
const Poly2Orient<dim> &o1,
const Poly2Orient<dim> &o2,
139 Poly2OrientIntersectData &data)
141 if(!o1.m_origin.isValid() || !o2.m_origin.isValid()) {
147 if(!o1.m_axes[0].isValid()) {
148 if(!o2.checkContained(o1.m_origin, data.p2))
153 data.p1[0] = data.p1[1] = 0;
158 if(!o2.m_axes[0].isValid()) {
159 if(!o1.checkContained(o2.m_origin, data.p1))
162 data.p2[0] = data.p2[1] = 0;
171 Vector<dim> basis1, basis2;
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]);
180 sqrmag1 = basis1.sqrMag();
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]);
191 basis2 -= basis1 * (Dot(basis1, basis2) / sqrmag1);
193 sqrmag2 = basis2.sqrMag();
195 if(basis_size++ == 0) {
202 Vector<dim> off = o2.m_origin - o1.m_origin;
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];
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];
228 if(off1 - off2 != off)
237 data.o1_is_line = !o1.m_axes[1].isValid();
238 data.o2_is_line = !o2.m_axes[1].isValid();
240 if(!o1.m_axes[1].isValid() && !o2.m_axes[1].isValid()) {
242 if(off != o2.m_axes[0] * proj)
247 data.p1[0] = data.p1[1] = 0;
248 data.v2[0] = (Dot(o1.m_axes[0], o2.m_axes[0]) > 0) ? 1 : -1;
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]);
260 if(off != - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
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]);
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]);
276 if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1])
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]);
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]);
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])
297 basis1 /= std::sqrt(sqrmag1);
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);
308 assert(o1.m_axes[1].isValid() && o2.m_axes[1].isValid());
311 CoordType off_sqr_mag = data.off.sqrMag();
315 if(off_sqr_mag != 0) {
316 Vector<dim> off_copy = off;
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];
327 data.off[0] = data.off[1] = 0;
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]);
344inline bool Intersect(
const Polygon<dim>& r,
const Point<dim>& p,
bool proper)
348 return r.m_poly.numCorners() > 0 && r.m_orient.checkContained(p, p2)
349 && Intersect(r.m_poly, p2, proper);
353inline bool Contains(
const Point<dim>& p,
const Polygon<dim>& r,
bool proper)
355 if(r.m_poly.numCorners() == 0)
361 for(
size_t i = 1; i < r.m_poly.numCorners(); ++i)
362 if(r.m_poly[i] != r.m_poly[0])
367 return r.m_orient.checkContained(p, p2) && p2 == r.m_poly[0];
371bool Intersect(
const Polygon<dim>& p,
const AxisBox<dim>& b,
bool proper)
373 size_t corners = p.m_poly.numCorners();
380 if(!p.m_orient.checkIntersect(b, p2, proper))
384 s.endpoint(0) = p.m_orient.convert(p.m_poly.getCorner(corners-1));
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))
391 next_end = next_end ? 0 : 1;
394 return Contains(p, p2, proper);
398bool _PolyContainsBox(
const Poly2Orient<dim> &orient,
const Polygon<2> &poly,
399 const Point<dim> &corner,
const Vector<dim> &size,
bool proper)
401 int num_dim = 0, nonzero_dim = -1;
403 for(
int i = 0; i < dim; ++i) {
408 if(nonzero_dim == -1 || std::fabs(size[nonzero_dim]) < std::fabs(size[i]))
415 if(!orient.checkContained(corner, corner1))
419 return Contains(poly, corner1, proper);
423 if(!orient.checkContained(corner + size, corner2))
427 return Contains(poly, Segment<2>(corner1, corner2), proper);
429 Point<dim> other_corner = corner;
430 other_corner[nonzero_dim] += size[nonzero_dim];
433 if(!orient.checkContained(other_corner, corner3))
438 Vector<2> vec1(corner2 - corner1), vec2(corner3 - corner1);
443 m.rotation(Vector<2>(1, 0), vec1);
445 catch(
const ColinearVectors<2>&) {
449 RotBox<2> box(corner1,
ProdInv(vec2, m), m);
451 return Contains(poly, box, proper);
455inline bool Contains(
const Polygon<dim>& p,
const AxisBox<dim>& b,
bool proper)
457 return _PolyContainsBox(p.m_orient, p.m_poly, b.m_low, b.m_high - b.m_low, proper);
461inline bool Contains(
const AxisBox<dim>& b,
const Polygon<dim>& p,
bool proper)
463 for(
size_t i = 0; i < p.m_poly.numCorners(); ++i)
464 if(!Contains(b, p.getCorner(i), proper))
471inline bool Intersect(
const Polygon<dim>& p,
const Ball<dim>& b,
bool proper)
473 if(p.m_poly.numCorners() == 0)
479 dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
481 if(_Less(dist, 0, proper))
484 return Intersect(p.m_poly, Ball<2>(c2, std::sqrt(dist)), proper);
488inline bool Contains(
const Polygon<dim>& p,
const Ball<dim>& b,
bool proper)
490 if(p.m_poly.numCorners() == 0)
498 if(!p.m_orient.checkContained(b.m_center, c2))
501 return Contains(p.m_poly, c2, proper);
505inline bool Contains(
const Ball<dim>& b,
const Polygon<dim>& p,
bool proper)
507 if(p.m_poly.numCorners() == 0)
513 dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
515 if(_Less(dist, 0, proper))
518 for(
size_t i = 0; i != p.m_poly.numCorners(); ++i)
519 if(_Less(dist, SquaredDistance(c2, p.m_poly[i]), proper))
526bool Intersect(
const Polygon<dim>& p,
const Segment<dim>& s,
bool proper)
528 if(p.m_poly.numCorners() == 0)
535 v1 = p.m_orient.offset(s.m_p1, p1);
536 v2 = p.m_orient.offset(s.m_p2, p2);
543 Point<2> p_intersect;
546 return Intersect(p.m_poly, Segment<2>(p1, p2), proper);
548 for(
int i = 0; i < 2; ++i)
549 p_intersect[i] = (p1[i] * d2 + p2[i] * d1) / (d1 + d2);
551 return Intersect(p.m_poly, p_intersect, proper);
555inline bool Contains(
const Polygon<dim>& p,
const Segment<dim>& s,
bool proper)
557 if(p.m_poly.numCorners() == 0)
562 if(!p.m_orient.checkContained(s.m_p1, s2.endpoint(0)))
564 if(!p.m_orient.checkContained(s.m_p2, s2.endpoint(1)))
567 return Contains(p.m_poly, s2, proper);
571inline bool Contains(
const Segment<dim>& s,
const Polygon<dim>& p,
bool proper)
573 if(p.m_poly.numCorners() == 0)
580 Poly2Orient<dim> orient(p.m_orient);
582 for(
int i = 0; i < 2; ++i)
583 if(!orient.expand(s.endpoint(i), s2.endpoint(i)))
586 return Contains(s2, p.m_poly, proper);
590bool Intersect(
const Polygon<dim>& p,
const RotBox<dim>& r,
bool proper)
592 size_t corners = p.m_poly.numCorners();
597 Poly2Orient<dim> orient(p.m_orient);
599 orient.rotate(r.m_orient.inverse(), r.m_corner0);
601 AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
605 if(!orient.checkIntersect(b, p2, proper))
609 s.endpoint(0) = orient.convert(p.m_poly.getCorner(corners-1));
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))
616 next_end = next_end ? 0 : 1;
619 return Contains(p, p2, proper);
623inline bool Contains(
const Polygon<dim>& p,
const RotBox<dim>& r,
bool proper)
625 Poly2Orient<dim> orient(p.m_orient);
626 orient.rotate(r.m_orient.inverse(), r.m_corner0);
628 return _PolyContainsBox(orient, p.m_poly, r.m_corner0, r.m_size, proper);
632inline bool Contains(
const RotBox<dim>& r,
const Polygon<dim>& p,
bool proper)
634 if(p.m_poly.numCorners() == 0)
637 AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
639 Poly2Orient<dim> orient(p.m_orient);
640 orient.rotate(r.m_orient.inverse(), r.m_corner0);
642 for(
size_t i = 0; i < p.m_poly.numCorners(); ++i)
643 if(!Contains(b, orient.convert(p.m_poly[i]), proper))
649bool PolyPolyIntersect(
const Polygon<2> &poly1,
const Polygon<2> &poly2,
651 const Poly2OrientIntersectData &data,
bool proper);
654inline bool Intersect(
const Polygon<dim>& p1,
const Polygon<dim>& p2,
bool proper)
656 Poly2OrientIntersectData data;
658 int intersect_dim = Intersect(p1.m_orient, p2.m_orient, data);
660 return PolyPolyIntersect(p1.m_poly, p2.m_poly, intersect_dim, data, proper);
663bool PolyPolyContains(
const Polygon<2> &outer,
const Polygon<2> &inner,
665 const Poly2OrientIntersectData &data,
bool proper);
668inline bool Contains(
const Polygon<dim>& outer,
const Polygon<dim>& inner,
bool proper)
670 if(outer.m_poly.numCorners() == 0)
671 return !proper && inner.m_poly.numCorners() == 0;
673 if(inner.m_poly.numCorners() == 0)
676 Poly2OrientIntersectData data;
678 int intersect_dim = Intersect(outer.m_orient, inner.m_orient, data);
680 return PolyPolyContains(outer.m_poly, inner.m_poly, intersect_dim, data, proper);
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);
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);
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);
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);
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);
716template bool Intersect<3>(
const Polygon<3>&,
const Polygon<3>&,
bool);
717template bool Contains<3>(
const Polygon<3>&,
const Polygon<3>&,
bool);
720bool Poly2Orient<3>::checkIntersectPlane(
const AxisBox<3>& b, Point<2>& p2,
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());
726 Vector<3> normal =
Cross(m_axes[0], m_axes[1]);
734 CoordType normal_mag = normal.sloppyMag();
735 int high_corner_num = 0;
737 for(
int i = 0; i < 3; ++i) {
738 if(std::fabs(normal[i]) < normal_mag * numeric_constants<CoordType>::epsilon()) {
740 }
else if(normal[i] > 0) {
742 high_corner_num |= (1 << i);
748 int low_corner_num = high_corner_num ^ 7;
750 Point<3> high_corner = b.getCorner(high_corner_num);
751 Point<3> low_corner = b.getCorner(low_corner_num);
755 CoordType perp_size = Dot(normal, high_corner - low_corner) / normal_mag;
756 assert(perp_size >= 0);
758 if(perp_size < normal_mag * numeric_constants<CoordType>::epsilon()) {
760 return !proper && checkContained(
Midpoint(high_corner, low_corner), p2);
763 if(_Less(Dot(high_corner - m_origin, normal), 0, proper)
764 || _Less(Dot(low_corner - m_origin, normal), 0, proper))
769 Point<2> p2_high, p2_low;
771 CoordType high_dist = offset(high_corner, p2_high).mag();
772 CoordType low_dist = offset(low_corner, p2_low).mag();
774 p2 =
Midpoint(p2_high, p2_low, high_dist / (high_dist + low_dist));
780static void LinePolyGetBounds(
const Polygon<2> &poly, CoordType &low, CoordType &high)
782 low = high = poly[0][0];
784 for(
size_t i = 0; i < poly.numCorners(); ++i) {
803 const Vector<2> &v, std::vector<CoordType> &cross,
806 assert(poly.numCorners() == cross.size());
807 assert(Equal(v.
sqrMag(), 1));
810 Point<2> old_p = poly.getCorner(poly.numCorners() - 1);
811 bool old_below = (
Cross(v, old_p - p) < 0);
815 std::list<LinePointData> line_point_data;
817 for(
size_t i = 0; i < poly.numCorners(); ++i) {
823 if(Equal(v_i_sqr_mag, proj * proj)) {
826 CoordType proj_j, low_proj = proj, high_proj = proj;
828 for(j = i + 1; j != i; j == poly.numCorners() - 1 ? j = 0 : ++j) {
829 p_j = poly.getCorner(j);
831 proj_j = Dot(v_j, v);
833 if(!Equal(v_j.
sqrMag(), proj_j * proj_j))
836 if(proj_j < low_proj)
838 if(proj_j > high_proj)
844 bool below = (Cross(v, v_j) < 0);
846 if(below == old_below && proper) {
853 if(below != old_below) {
855 cross[next_cross++] = proj;
860 cross[next_cross++] = proj;
861 cross[next_cross++] = proj;
868 LinePointData data = {low_proj, high_proj, below != old_below};
870 std::list<LinePointData>::iterator I;
872 for(I = line_point_data.begin(); I != line_point_data.end(); ++I) {
873 if(data.low > I->high)
876 if(data.high < I->low) {
877 line_point_data.insert(I, data);
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);
891 if(J->low < I->high) {
893 I->cross = (I->cross != J->cross);
894 line_point_data.erase(J);
898 if(I == line_point_data.end())
899 line_point_data.push_back(data);
908 bool below = (
Cross(v, v_i) < 0);
910 if(below != old_below) {
912 Vector<2> dist = p - old_p;
916 CoordType denom = dist_proj * dist_proj - dist_sqr_mag;
920 CoordType line_pos = (dist_proj * Dot(v_i, dist) + dist_sqr_mag * proj) / denom;
922 cross[next_cross++] = line_pos;
928 cross.resize(next_cross);
929 std::sort(cross.begin(), cross.end());
931 if(!line_point_data.empty()) {
932 auto I = line_point_data.begin();
933 auto cross_num = cross.begin();
936 while(cross_num != cross.end() && I != line_point_data.end()) {
937 if(*cross_num < I->low) {
944 if(*cross_num > I->high) {
945 hit_between = I->cross;
948 auto high_cross_num = cross_num;
952 }
while(*high_cross_num < I->high);
954 hit_between = (((high_cross_num - cross_num) % 2) != 0) != I->cross;
956 cross_num = cross.erase(cross_num, high_cross_num);
960 cross_num = cross.insert(cross_num, proper == hit ? I->low : I->high);
965 cross_num = cross.insert(cross_num, I->low);
967 cross_num = cross.insert(cross_num, I->high);
973 while(I != line_point_data.end()) {
975 cross.push_back(proper == hit ? I->low : I->high);
979 cross.push_back(I->low);
980 cross.push_back(I->high);
988 return !cross.empty();
991bool PolyPolyIntersect(
const Polygon<2> &poly1,
const Polygon<2> &poly2,
992 const int intersect_dim,
993 const Poly2OrientIntersectData &data,
bool proper)
995 switch(intersect_dim) {
999 return Intersect(poly1, data.p1, proper)
1000 && Intersect(poly2, data.p2, proper);
1002 if(proper && (data.o1_is_line || data.o2_is_line))
1005 if(data.o1_is_line && data.o2_is_line) {
1009 LinePolyGetBounds(poly1, low1, high1);
1012 high1 -= data.p1[0];
1014 if(data.v1[0] < 0) {
1020 LinePolyGetBounds(poly2, low2, high2);
1023 high2 -= data.p2[0];
1025 if(data.v2[0] < 0) {
1031 return high1 >= low2 && high2 >= low1;
1034 if(data.o1_is_line) {
1037 LinePolyGetBounds(poly1, min, max);
1042 if(data.v1[0] < 0) {
1048 Segment<2> s(data.p2 + min * data.v2, data.p1 + max * data.v2);
1050 return Intersect(poly2, s,
false);
1053 if(data.o2_is_line) {
1056 LinePolyGetBounds(poly2, min, max);
1061 if(data.v2[0] < 0) {
1067 Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
1069 return Intersect(poly1, s,
false);
1073 std::vector<CoordType> cross1(poly1.numCorners());
1074 if(!GetCrossings(poly1, data.p1, data.v1, cross1, proper))
1077 std::vector<CoordType> cross2(poly2.numCorners());
1078 if(!GetCrossings(poly2, data.p2, data.v2, cross2, proper))
1081 auto i1 = cross1.begin(), i2 = cross2.begin();
1082 bool hit1 =
false, hit2 =
false;
1084 while(i1 != cross1.end() && i2 != cross2.end()) {
1085 if(Equal(*i1, *i2)) {
1115 Polygon<2> tmp_poly(poly2);
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;
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];
1125 return Intersect(poly1, tmp_poly, proper);
1133bool PolyPolyContains(
const Polygon<2> &outer,
const Polygon<2> &inner,
1134 const int intersect_dim,
1135 const Poly2OrientIntersectData &data,
bool proper)
1137 switch(intersect_dim) {
1141 return Contains(data.p2, inner,
false)
1142 && Contains(outer, data.p1, proper);
1144 if(!data.o2_is_line)
1150 LinePolyGetBounds(inner, min, max);
1155 if(data.v2[0] < 0) {
1161 Segment<2> s(data.p1 + min * data.v1, data.p1 + max * data.v1);
1163 return Contains(outer, s, proper);
1170 Polygon<2> tmp_poly(inner);
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;
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];
1180 return Contains(outer, tmp_poly, proper);
1196bool Intersect<2>(
const Polygon<2>& r,
const Point<2>& p,
bool proper)
1198 const Polygon<2>::theConstIter begin = r.m_points.begin(), end = r.m_points.end();
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]));
1206 if (!vertically_between)
1209 CoordType x_intersect = (*i)[0] + ((*j)[0] - (*i)[0])
1210 * (p[1] - (*i)[1]) / ((*j)[1] - (*i)[1]);
1212 if(Equal(p[0], x_intersect))
1215 if(p[0] < x_intersect)
1223bool Contains<2>(
const Point<2>& p,
const Polygon<2>& r,
bool proper)
1226 return r.numCorners() == 0;
1228 for(
const auto & point : r.m_points)
1236bool Intersect<2>(
const Polygon<2>& p,
const AxisBox<2>& b,
bool proper)
1238 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
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]));
1258 if(low_vertically_between) {
1259 CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
1262 if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
1264 if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1268 if(b.m_low[0] < x_intersect)
1272 if(low_horizontally_between) {
1273 CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
1276 if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
1278 if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1282 if(high_vertically_between) {
1283 CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
1286 if(Equal(b.m_low[0], x_intersect) || Equal(b.m_high[0], x_intersect))
1288 if(b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1292 if(high_horizontally_between) {
1293 CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
1296 if(Equal(b.m_low[1], y_intersect) || Equal(b.m_high[1], y_intersect))
1298 if(b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1307bool Contains<2>(
const Polygon<2>& p,
const AxisBox<2>& b,
bool proper)
1309 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
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]));
1329 if(low_vertically_between) {
1330 CoordType x_intersect = (*i)[0] + (b.m_low[1] - (*i)[1])
1333 bool on_corner = Equal(b.m_low[0], x_intersect)
1334 || Equal(b.m_high[0], x_intersect);
1336 if(on_corner && proper)
1339 if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1343 if(!on_corner && b.m_low[0] < x_intersect)
1347 if(low_horizontally_between) {
1348 CoordType y_intersect = (*i)[1] + (b.m_low[0] - (*i)[0])
1351 bool on_corner = Equal(b.m_low[1], y_intersect)
1352 || Equal(b.m_high[1], y_intersect);
1354 if(on_corner && proper)
1357 if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1361 if(high_vertically_between) {
1362 CoordType x_intersect = (*i)[0] + (b.m_high[1] - (*i)[1])
1365 bool on_corner = Equal(b.m_low[0], x_intersect)
1366 || Equal(b.m_high[0], x_intersect);
1368 if(on_corner && proper)
1371 if(!on_corner && b.m_low[0] < x_intersect && b.m_high[0] > x_intersect)
1375 if(high_horizontally_between) {
1376 CoordType y_intersect = (*i)[1] + (b.m_high[0] - (*i)[0])
1379 bool on_corner = Equal(b.m_low[1], y_intersect)
1380 || Equal(b.m_high[1], y_intersect);
1382 if(on_corner && proper)
1385 if(!on_corner && b.m_low[1] < y_intersect && b.m_high[1] > y_intersect)
1394bool Contains<2>(
const AxisBox<2>& b,
const Polygon<2>& p,
bool proper)
1396 for(
const auto & point : p.m_points)
1397 if(!Contains(b, point, proper))
1404bool Intersect<2>(
const Polygon<2>& p,
const Ball<2>& b,
bool proper)
1406 if(Contains(p, b.m_center, proper))
1410 s2.endpoint(0) = p.m_points.back();
1413 for(
const auto & point : p.m_points) {
1414 s2.endpoint(next_end) = point;
1415 if(Intersect(s2, b, proper))
1417 next_end = next_end ? 0 : 1;
1424bool Contains<2>(
const Polygon<2>& p,
const Ball<2>& b,
bool proper)
1426 if(!Contains(p, b.m_center, proper))
1430 s2.endpoint(0) = p.m_points.back();
1433 for(
const auto & point : p.m_points) {
1434 s2.endpoint(next_end) = point;
1435 if(Intersect(s2, b, !proper))
1437 next_end = next_end ? 0 : 1;
1444bool Contains<2>(
const Ball<2>& b,
const Polygon<2>& p,
bool proper)
1446 CoordType sqr_dist = b.m_radius * b.m_radius;
1448 for(
const auto & point : p.m_points)
1449 if(_Greater(SquaredDistance(b.m_center, point), sqr_dist, proper))
1456bool Intersect<2>(
const Polygon<2>& p,
const Segment<2>& s,
bool proper)
1458 if(Contains(p, s.endpoint(0), proper))
1461 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1465 s2.endpoint(0) = p.m_points.back();
1468 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1469 s2.endpoint(next_point) = *i;
1470 if(Intersect(s, s2, proper))
1472 next_point = next_point ? 0 : 1;
1479bool Contains<2>(
const Polygon<2>& p,
const Segment<2>& s,
bool proper)
1481 if(proper && !Contains(p, s.endpoint(0),
true))
1484 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1488 s2.endpoint(0) = p.m_points.back();
1492 for(Polygon<2>::theConstIter i = begin; i != end; ++i) {
1493 s2.endpoint(next_point) = *i;
1494 if(Intersect(s2, s, !proper))
1496 bool this_point = next_point;
1497 next_point = next_point ? 0 : 1;
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);
1505 Vector<2> edge2 = *i - *(i + 1);
1511 if(edge1[1] * edge2[1] > 0
1512 || ((edge1[1] > 0) ? c1 : c2) < 0)
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]));
1530 if (!vertically_between)
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]);
1537 if(Equal(s.m_p1[0], x_intersect)) {
1540 if(s2.endpoint(next_point) == s.m_p1)
1542 assert(s2.endpoint(this_point) != s.m_p1);
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;
1548 if(
Cross(segment, poly_edge) < 0)
1551 else if(s.m_p1[0] < x_intersect)
1555 return proper || hit;
1559bool Contains<2>(
const Segment<2>& s,
const Polygon<2>& p,
bool proper)
1561 for(
const auto & point : p.m_points)
1562 if(!Contains(s, point, proper))
1569bool Intersect<2>(
const Polygon<2>& p,
const RotBox<2>& r,
bool proper)
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];
1579 m_high[j] = r.m_corner0[j];
1580 m_low[j] = r.m_corner0[j] + r.m_size[j];
1585 ends[0] = p.m_points.back();
1587 ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1590 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1593 for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1594 ends[next_end] = *i;
1596 ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1597 next_end = next_end ? 0 : 1;
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]));
1612 CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1613 CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1615 if(low_vertically_between) {
1616 CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1619 if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
1621 if(m_low[0] < x_intersect && m_high[0] > x_intersect)
1625 if(m_low[0] < x_intersect)
1629 if(low_horizontally_between) {
1630 CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1633 if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1635 if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1639 if(high_vertically_between) {
1640 CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1643 if(Equal(m_low[0], x_intersect) || Equal(m_high[0], x_intersect))
1645 if(m_low[0] < x_intersect && m_high[0] > x_intersect)
1649 if(high_horizontally_between) {
1650 CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1653 if(Equal(m_low[1], y_intersect) || Equal(m_high[1], y_intersect))
1655 if(m_low[1] < y_intersect && m_high[1] > y_intersect)
1664bool Contains<2>(
const Polygon<2>& p,
const RotBox<2>& r,
bool proper)
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];
1674 m_high[j] = r.m_corner0[j];
1675 m_low[j] = r.m_corner0[j] + r.m_size[j];
1680 ends[0] = p.m_points.back();
1682 ends[0].rotate(r.m_orient.inverse(), r.m_corner0);
1685 const Polygon<2>::theConstIter begin = p.m_points.begin(), end = p.m_points.end();
1688 for (Polygon<2>::theConstIter i = begin; i != end; ++i) {
1689 ends[next_end] = *i;
1691 ends[next_end].rotate(r.m_orient.inverse(), r.m_corner0);
1692 next_end = next_end ? 0 : 1;
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]));
1707 CoordType xdiff = (ends[1])[0] - (ends[0])[0];
1708 CoordType ydiff = (ends[1])[1] - (ends[0])[1];
1710 if(low_vertically_between) {
1711 CoordType x_intersect = (ends[0])[0] + (m_low[1] - (ends[0])[1])
1714 bool on_corner = Equal(m_low[0], x_intersect)
1715 || Equal(m_high[0], x_intersect);
1717 if(on_corner && proper)
1720 if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1724 if(!on_corner && m_low[0] < x_intersect)
1728 if(low_horizontally_between) {
1729 CoordType y_intersect = (ends[0])[1] + (m_low[0] - (ends[0])[0])
1732 bool on_corner = Equal(m_low[1], y_intersect)
1733 || Equal(m_high[1], y_intersect);
1735 if(on_corner && proper)
1738 if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1742 if(high_vertically_between) {
1743 CoordType x_intersect = (ends[0])[0] + (m_high[1] - (ends[0])[1])
1746 bool on_corner = Equal(m_low[0], x_intersect)
1747 || Equal(m_high[0], x_intersect);
1749 if(on_corner && proper)
1752 if(!on_corner && m_low[0] < x_intersect && m_high[0] > x_intersect)
1756 if(high_horizontally_between) {
1757 CoordType y_intersect = (ends[0])[1] + (m_high[0] - (ends[0])[0])
1760 bool on_corner = Equal(m_low[1], y_intersect)
1761 || Equal(m_high[1], y_intersect);
1763 if(on_corner && proper)
1766 if(!on_corner && m_low[1] < y_intersect && m_high[1] > y_intersect)
1775bool Contains<2>(
const RotBox<2>& r,
const Polygon<2>& p,
bool proper)
1777 for(
const auto & point : p.m_points)
1778 if(!Contains(r, point, proper))
1785bool Intersect<2>(
const Polygon<2>& p1,
const Polygon<2>& p2,
bool proper)
1787 auto begin1 = p1.m_points.begin(), end1 = p1.m_points.end();
1788 auto begin2 = p2.m_points.begin(), end2 = p2.m_points.end();
1790 int next_end1, next_end2;
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;
1799 for(
auto i2 = begin2; i2 != end2; ++i2) {
1800 s2.endpoint(next_end2) = *i2;
1801 next_end2 = next_end2 ? 0 : 1;
1803 if(Intersect(s1, s2, proper))
1808 return Contains(p1, p2.m_points.front(), proper)
1809 || Contains(p2, p1.m_points.front(), proper);
1813bool Contains<2>(
const Polygon<2>& outer,
const Polygon<2>& inner,
bool proper)
1815 if(proper && !Contains(outer, inner.m_points.front(),
true))
1818 auto begin = inner.m_points.begin(), end = inner.m_points.end();
1820 s.endpoint(0) = inner.m_points.back();
1823 for(
auto i = begin; i != end; ++i) {
1824 s.endpoint(next_end) = *i;
1825 next_end = next_end ? 0 : 1;
1827 if(!Contains(outer, s,
false))
1831 auto begin2 = outer.m_points.begin(),
1832 end2 = outer.m_points.end();
1834 s2.endpoint(0) = outer.m_points.back();
1836 for(
auto i2 = begin2; i2 != end2; ++i2) {
1837 s2.endpoint(next_end2) = *i2;
1838 next_end2 = next_end2 ? 0 : 1;
1840 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.