27 #ifndef WFMATH_POLYGON_INTERSECT_H
28 #define WFMATH_POLYGON_INTERSECT_H
30 #include <wfmath/axisbox.h>
31 #include <wfmath/ball.h>
32 #include <wfmath/polygon.h>
33 #include <wfmath/intersect.h>
34 #include <wfmath/error.h>
48 inline Vector<dim> _Poly2Orient<dim>::offset(
const Point<dim>& pd, Point<2>& p2)
const
50 assert(m_origin.isValid());
52 Vector<dim> out = pd - m_origin;
54 for(
int j = 0; j < 2; ++j) {
55 p2[j] = Dot(out, m_axes[j]);
56 out -= p2[j] * m_axes[j];
63 inline bool _Poly2Orient<dim>::checkContained(
const Point<dim>& pd, Point<2> & p2)
const
65 Vector<dim> off = offset(pd, p2);
68 for(
int i = 0; i < dim; ++i)
69 sqrsum += pd[i] * pd[i];
71 return off.sqrMag() < numeric_constants<CoordType>::epsilon() * sqrsum;
75 bool _Poly2Orient<3>::checkIntersectPlane(
const AxisBox<3>& b, Point<2>& p2,
79 bool _Poly2Orient<dim>::checkIntersect(
const AxisBox<dim>& b, Point<2>& p2,
82 assert(m_origin.isValid());
84 if(!m_axes[0].isValid()) {
87 return Intersect(b, convert(p2), proper);
90 if(m_axes[1].isValid()) {
96 return checkIntersectPlane(b, p2, proper);
104 bool got_bounds =
false;
106 for(
int i = 0; i < dim; ++i) {
109 if(_Less(m_origin[i], b.lowCorner()[i], proper)
110 || _Greater(m_origin[i], b.highCorner()[i], proper))
114 CoordType low = (b.lowCorner()[i] - m_origin[i]) / dist;
115 CoordType high = (b.highCorner()[i] - m_origin[i]) / dist;
137 if(_LessEq(min, max, proper)) {
138 p2[0] = (max - min) / 2;
147 int _Intersect(
const _Poly2Orient<dim> &o1,
const _Poly2Orient<dim> &o2,
148 _Poly2OrientIntersectData &data)
150 if(!o1.m_origin.isValid() || !o2.m_origin.isValid()) {
156 if(!o1.m_axes[0].isValid()) {
157 if(!o2.checkContained(o1.m_origin, data.p2))
160 _Poly2OrientIntersectData data;
162 data.p1[0] = data.p1[1] = 0;
167 if(!o2.m_axes[0].isValid()) {
168 if(!o1.checkContained(o2.m_origin, data.p1))
171 data.p2[0] = data.p2[1] = 0;
180 Vector<dim> basis1, basis2;
184 basis1 = o2.m_axes[0] * Dot(o2.m_axes[0], o1.m_axes[0]);
185 if(o2.m_axes[1].isValid())
186 basis1 += o2.m_axes[1] * Dot(o2.m_axes[1], o1.m_axes[0]);
189 sqrmag1 = basis1.sqrMag();
190 if(sqrmag1 > numeric_constants<CoordType>::epsilon() * numeric_constants<CoordType>::epsilon())
193 if(o1.m_axes[1].isValid()) {
194 basis2 = o2.m_axes[0] * Dot(o2.m_axes[0], o1.m_axes[1]);
195 if(o2.m_axes[1].isValid())
196 basis2 += o2.m_axes[1] * Dot(o2.m_axes[1], o1.m_axes[1]);
200 basis2 -= basis1 * (Dot(basis1, basis2) / sqrmag1);
202 sqrmag2 = basis2.sqrMag();
203 if(sqrmag2 > numeric_constants<CoordType>::epsilon() * numeric_constants<CoordType>::epsilon()) {
204 if(basis_size++ == 0) {
211 Vector<dim> off = o2.m_origin - o1.m_origin;
219 data.p1[0] = Dot(o1.m_axes[0], off);
220 Vector<dim> off1 = o1.m_axes[0] * data.p1[0];
221 if(o1.m_axes[1].isValid()) {
222 data.p1[1] = Dot(o1.m_axes[1], off);
223 off1 += o1.m_axes[1] * data.p1[1];
228 data.p2[0] = -Dot(o2.m_axes[0], off);
229 Vector<dim> off2 = o2.m_axes[0] * data.p2[0];
230 if(o1.m_axes[1].isValid()) {
231 data.p2[1] = -Dot(o2.m_axes[1], off);
232 off2 += o1.m_axes[1] * data.p2[1];
237 if(off1 - off2 != off)
246 data.o1_is_line = !o1.m_axes[1].isValid();
247 data.o2_is_line = !o2.m_axes[1].isValid();
249 if(!o1.m_axes[1].isValid() && !o2.m_axes[1].isValid()) {
251 if(off != o2.m_axes[0] * proj)
256 data.p1[0] = data.p1[1] = 0;
257 data.v2[0] = (Dot(o1.m_axes[0], o2.m_axes[0]) > 0) ? 1 : -1;
265 if(!o1.m_axes[1].isValid()) {
266 data.p2[0] = -Dot(off, o2.m_axes[0]);
267 data.p2[1] = -Dot(off, o2.m_axes[1]);
269 if(off != - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
274 data.p1[0] = data.p1[1] = 0;
275 data.v2[0] = Dot(o1.m_axes[0], o2.m_axes[0]);
276 data.v2[1] = Dot(o1.m_axes[0], o2.m_axes[1]);
281 if(!o2.m_axes[1].isValid()) {
282 data.p1[0] = Dot(off, o1.m_axes[0]);
283 data.p1[1] = Dot(off, o1.m_axes[1]);
285 if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1])
290 data.p2[0] = data.p2[1] = 0;
291 data.v1[0] = Dot(o1.m_axes[0], o2.m_axes[0]);
292 data.v1[1] = Dot(o1.m_axes[1], o2.m_axes[0]);
297 data.p1[0] = Dot(off, o1.m_axes[0]);
298 data.p1[1] = Dot(off, o1.m_axes[1]);
299 data.p2[0] = -Dot(off, o2.m_axes[0]);
300 data.p2[1] = -Dot(off, o2.m_axes[1]);
302 if(off != data.p1[0] * o1.m_axes[0] + data.p1[1] * o1.m_axes[1]
303 - data.p2[0] * o2.m_axes[0] - data.p2[1] * o2.m_axes[1])
306 basis1 /= std::sqrt(sqrmag1);
308 data.v1[0] = Dot(o1.m_axes[0], basis1);
309 data.v1[1] = Dot(o1.m_axes[1], basis1);
310 data.v2[0] = Dot(o2.m_axes[0], basis1);
311 data.v2[1] = Dot(o2.m_axes[1], basis1);
317 assert(o1.m_axes[1].isValid() && o2.m_axes[1].isValid());
320 CoordType off_sqr_mag = data.off.sqrMag();
324 if(off_sqr_mag != 0) {
325 Vector<dim> off_copy = off;
327 data.off[0] = Dot(o2.m_axes[0], off);
328 off_copy -= o1.m_axes[0] * data.off[0];
329 data.off[1] = Dot(o2.m_axes[1], off);
330 off_copy -= o1.m_axes[1] * data.off[1];
332 if(off_copy.sqrMag() > off_sqr_mag * numeric_constants<CoordType>::epsilon())
336 data.off[0] = data.off[1] = 0;
339 data.v1[0] = Dot(o2.m_axes[0], o1.m_axes[0]);
340 data.v1[1] = Dot(o2.m_axes[0], o1.m_axes[1]);
341 data.v2[0] = Dot(o2.m_axes[1], o1.m_axes[0]);
342 data.v2[1] = Dot(o2.m_axes[1], o1.m_axes[1]);
353 inline bool Intersect(
const Polygon<dim>& r,
const Point<dim>& p,
bool proper)
357 return r.m_poly.numCorners() > 0 && r.m_orient.checkContained(p, p2)
358 && Intersect(r.m_poly, p2, proper);
362 inline bool Contains(
const Point<dim>& p,
const Polygon<dim>& r,
bool proper)
364 if(r.m_poly.numCorners() == 0)
370 for(
size_t i = 1; i < r.m_poly.numCorners(); ++i)
371 if(r.m_poly[i] != r.m_poly[0])
376 return r.m_orient.checkContained(p, p2) && p2 == r.m_poly[0];
380 bool Intersect(
const Polygon<dim>& p,
const AxisBox<dim>& b,
bool proper)
382 size_t corners = p.m_poly.numCorners();
389 if(!p.m_orient.checkIntersect(b, p2, proper))
393 s.endpoint(0) = p.m_orient.convert(p.m_poly.getCorner(corners-1));
396 for(
size_t i = 0; i < corners; ++i) {
397 s.endpoint(next_end) = p.m_orient.convert(p.m_poly.getCorner(i));
398 if(Intersect(b, s, proper))
400 next_end = next_end ? 0 : 1;
403 return Contains(p, p2, proper);
407 bool _PolyContainsBox(
const _Poly2Orient<dim> &orient,
const Polygon<2> &poly,
408 const Point<dim> &corner,
const Vector<dim> &size,
bool proper)
410 int num_dim = 0, nonzero_dim = -1;
412 for(
int i = 0; i < dim; ++i) {
417 if(nonzero_dim == -1 || std::fabs(size[nonzero_dim]) < std::fabs(size[i]))
424 if(!orient.checkContained(corner, corner1))
428 return Contains(poly, corner1, proper);
432 if(!orient.checkContained(corner + size, corner2))
436 return Contains(poly, Segment<2>(corner1, corner2), proper);
438 Point<dim> other_corner = corner;
439 other_corner[nonzero_dim] += size[nonzero_dim];
442 if(!orient.checkContained(other_corner, corner3))
447 Vector<2> vec1(corner2 - corner1), vec2(corner3 - corner1);
452 m.rotation(Vector<2>(1, 0), vec1);
454 catch(ColinearVectors<2>) {
458 RotBox<2> box(corner1,
ProdInv(vec2, m), m);
460 return Contains(poly, box, proper);
464 inline bool Contains(
const Polygon<dim>& p,
const AxisBox<dim>& b,
bool proper)
466 return _PolyContainsBox(p.m_orient, p.m_poly, b.m_low, b.m_high - b.m_low, proper);
470 inline bool Contains(
const AxisBox<dim>& b,
const Polygon<dim>& p,
bool proper)
472 for(
size_t i = 0; i < p.m_poly.numCorners(); ++i)
473 if(!Contains(b, p.getCorner(i), proper))
480 inline bool Intersect(
const Polygon<dim>& p,
const Ball<dim>& b,
bool proper)
482 if(p.m_poly.numCorners() == 0)
488 dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
490 if(_Less(dist, 0, proper))
493 return Intersect(p.m_poly, Ball<2>(c2, std::sqrt(dist)), proper);
497 inline bool Contains(
const Polygon<dim>& p,
const Ball<dim>& b,
bool proper)
499 if(p.m_poly.numCorners() == 0)
507 if(!p.m_orient.checkContained(b.m_center, c2))
510 return Contains(p.m_poly, c2, proper);
514 inline bool Contains(
const Ball<dim>& b,
const Polygon<dim>& p,
bool proper)
516 if(p.m_poly.numCorners() == 0)
522 dist = b.m_radius * b.m_radius - p.m_orient.offset(b.m_center, c2).sqrMag();
524 if(_Less(dist, 0, proper))
527 for(
size_t i = 0; i != p.m_poly.numCorners(); ++i)
528 if(_Less(dist, SquaredDistance(c2, p.m_poly[i]), proper))
535 bool Intersect(
const Polygon<dim>& p,
const Segment<dim>& s,
bool proper)
537 if(p.m_poly.numCorners() == 0)
544 v1 = p.m_orient.offset(s.m_p1, p1);
545 v2 = p.m_orient.offset(s.m_p2, p2);
552 Point<2> p_intersect;
555 return Intersect(p.m_poly, Segment<2>(p1, p2), proper);
557 for(
int i = 0; i < 2; ++i)
558 p_intersect[i] = (p1[i] * d2 + p2[i] * d1) / (d1 + d2);
560 return Intersect(p.m_poly, p_intersect, proper);
564 inline bool Contains(
const Polygon<dim>& p,
const Segment<dim>& s,
bool proper)
566 if(p.m_poly.numCorners() == 0)
571 if(!p.m_orient.checkContained(s.m_p1, s2.endpoint(0)))
573 if(!p.m_orient.checkContained(s.m_p2, s2.endpoint(1)))
576 return Contains(p.m_poly, s2, proper);
580 inline bool Contains(
const Segment<dim>& s,
const Polygon<dim>& p,
bool proper)
582 if(p.m_poly.numCorners() == 0)
589 _Poly2Orient<dim> orient(p.m_orient);
591 for(
int i = 0; i < 2; ++i)
592 if(!orient.expand(s.endpoint(i), s2.endpoint(i)))
595 return Contains(s2, p.m_poly, proper);
599 bool Intersect(
const Polygon<dim>& p,
const RotBox<dim>& r,
bool proper)
601 size_t corners = p.m_poly.numCorners();
606 _Poly2Orient<dim> orient(p.m_orient);
608 orient.rotate(r.m_orient.inverse(), r.m_corner0);
610 AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
614 if(!orient.checkIntersect(b, p2, proper))
618 s.endpoint(0) = orient.convert(p.m_poly.getCorner(corners-1));
621 for(
size_t i = 0; i < corners; ++i) {
622 s.endpoint(next_end) = orient.convert(p.m_poly.getCorner(i));
623 if(Intersect(b, s, proper))
625 next_end = next_end ? 0 : 1;
628 return Contains(p, p2, proper);
632 inline bool Contains(
const Polygon<dim>& p,
const RotBox<dim>& r,
bool proper)
634 _Poly2Orient<dim> orient(p.m_orient);
635 orient.rotate(r.m_orient.inverse(), r.m_corner0);
637 return _PolyContainsBox(orient, p.m_poly, r.m_corner0, r.m_size, proper);
641 inline bool Contains(
const RotBox<dim>& r,
const Polygon<dim>& p,
bool proper)
643 if(p.m_poly.numCorners() == 0)
646 AxisBox<dim> b(r.m_corner0, r.m_corner0 + r.m_size);
648 _Poly2Orient<dim> orient(p.m_orient);
649 orient.rotate(r.m_orient.inverse(), r.m_corner0);
651 for(
size_t i = 0; i < p.m_poly.numCorners(); ++i)
652 if(!Contains(b, orient.convert(p.m_poly[i]), proper))
658 bool _PolyPolyIntersect(
const Polygon<2> &poly1,
const Polygon<2> &poly2,
659 const int intersect_dim,
660 const _Poly2OrientIntersectData &data,
bool proper);
663 inline bool Intersect(
const Polygon<dim>& p1,
const Polygon<dim>& p2,
bool proper)
665 _Poly2OrientIntersectData data;
667 int intersect_dim = _Intersect(p1.m_orient, p2.m_orient, data);
669 return _PolyPolyIntersect(p1.m_poly, p2.m_poly, intersect_dim, data, proper);
672 bool _PolyPolyContains(
const Polygon<2> &outer,
const Polygon<2> &inner,
673 const int intersect_dim,
674 const _Poly2OrientIntersectData &data,
bool proper);
677 inline bool Contains(
const Polygon<dim>& outer,
const Polygon<dim>& inner,
bool proper)
679 if(outer.m_poly.numCorners() == 0)
680 return !proper && inner.m_poly.numCorners() == 0;
682 if(inner.m_poly.numCorners() == 0)
685 _Poly2OrientIntersectData data;
687 int intersect_dim = _Intersect(outer.m_orient, inner.m_orient, data);
689 return _PolyPolyContains(outer.m_poly, inner.m_poly, intersect_dim, data, proper);
693 bool Intersect(
const Polygon<2>& r,
const Point<2>& p,
bool proper);
695 bool Contains(
const Point<2>& p,
const Polygon<2>& r,
bool proper);
698 bool Intersect(
const Polygon<2>& p,
const AxisBox<2>& b,
bool proper);
700 bool Contains(
const Polygon<2>& p,
const AxisBox<2>& b,
bool proper);
702 bool Contains(
const AxisBox<2>& b,
const Polygon<2>& p,
bool proper);
705 bool Intersect(
const Polygon<2>& p,
const Ball<2>& b,
bool proper);
707 bool Contains(
const Polygon<2>& p,
const Ball<2>& b,
bool proper);
709 bool Contains(
const Ball<2>& b,
const Polygon<2>& p,
bool proper);
712 bool Intersect(
const Polygon<2>& r,
const Segment<2>& s,
bool proper);
714 bool Contains(
const Polygon<2>& p,
const Segment<2>& s,
bool proper);
716 bool Contains(
const Segment<2>& s,
const Polygon<2>& p,
bool proper);
719 bool Intersect(
const Polygon<2>& p,
const RotBox<2>& r,
bool proper);
721 bool Contains(
const Polygon<2>& p,
const RotBox<2>& r,
bool proper);
723 bool Contains(
const RotBox<2>& r,
const Polygon<2>& p,
bool proper);
726 bool Intersect(
const Polygon<2>& p1,
const Polygon<2>& p2,
bool proper);
728 bool Contains(
const Polygon<2>& outer,
const Polygon<2>& inner,
bool proper);
732 #endif // WFMATH_POLYGON_INTERSECT_H