24 #ifndef WFMATH_INTERSECT_H
25 #define WFMATH_INTERSECT_H
27 #include <wfmath/vector.h>
28 #include <wfmath/point.h>
29 #include <wfmath/const.h>
30 #include <wfmath/intersect_decls.h>
31 #include <wfmath/axisbox.h>
32 #include <wfmath/ball.h>
33 #include <wfmath/segment.h>
34 #include <wfmath/rotbox.h>
44 template<
class S1,
class S2>
45 inline bool Intersect(
const S1& s1,
const S2& s2,
bool proper)
47 return Intersect(s2, s1, proper);
53 inline bool Intersect(
const Point<dim>& p1,
const Point<dim>& p2,
bool proper)
55 return !proper && p1 == p2;
58 template<
int dim,
class S>
59 inline bool Contains(
const S& s,
const Point<dim>& p,
bool proper)
61 return Intersect(p, s, proper);
65 inline bool Contains(
const Point<dim>& p1,
const Point<dim>& p2,
bool proper)
67 return !proper && p1 == p2;
73 inline bool Intersect(
const AxisBox<dim>& b,
const Point<dim>& p,
bool proper)
75 for(
int i = 0; i < dim; ++i)
76 if(_Greater(b.m_low[i], p[i], proper) || _Less(b.m_high[i], p[i], proper))
83 inline bool Contains(
const Point<dim>& p,
const AxisBox<dim>& b,
bool proper)
85 return !proper && p == b.m_low && p == b.m_high;
89 inline bool Intersect(
const AxisBox<dim>& b1,
const AxisBox<dim>& b2,
bool proper)
91 for(
int i = 0; i < dim; ++i)
92 if(_Greater(b1.m_low[i], b2.m_high[i], proper)
93 || _Less(b1.m_high[i], b2.m_low[i], proper))
100 inline bool Contains(
const AxisBox<dim>& outer,
const AxisBox<dim>& inner,
bool proper)
102 for(
int i = 0; i < dim; ++i)
103 if(_Less(inner.m_low[i], outer.m_low[i], proper)
104 || _Greater(inner.m_high[i], outer.m_high[i], proper))
113 inline bool Intersect(
const Ball<dim>& b,
const Point<dim>& p,
bool proper)
115 return _LessEq(SquaredDistance(b.m_center, p), b.m_radius * b.m_radius
116 * (1 + numeric_constants<CoordType>::epsilon()), proper);
120 inline bool Contains(
const Point<dim>& p,
const Ball<dim>& b,
bool proper)
122 return !proper && b.m_radius == 0 && p == b.m_center;
126 inline bool Intersect(
const Ball<dim>& b,
const AxisBox<dim>& a,
bool proper)
130 for(
int i = 0; i < dim; ++i) {
132 if(b.m_center[i] < a.m_low[i])
133 dist_i = b.m_center[i] - a.m_low[i];
134 else if(b.m_center[i] > a.m_high[i])
135 dist_i = b.m_center[i] - a.m_high[i];
138 dist+= dist_i * dist_i;
141 return _LessEq(dist, b.m_radius * b.m_radius, proper);
145 inline bool Contains(
const Ball<dim>& b,
const AxisBox<dim>& a,
bool proper)
149 for(
int i = 0; i < dim; ++i) {
150 CoordType furthest = FloatMax(std::fabs(b.m_center[i] - a.m_low[i]),
151 std::fabs(b.m_center[i] - a.m_high[i]));
152 sqr_dist += furthest * furthest;
155 return _LessEq(sqr_dist, b.m_radius * b.m_radius * (1 + numeric_constants<CoordType>::epsilon()), proper);
159 inline bool Contains(
const AxisBox<dim>& a,
const Ball<dim>& b,
bool proper)
161 for(
int i = 0; i < dim; ++i)
162 if(_Less(b.m_center[i] - b.m_radius, a.lowerBound(i), proper)
163 || _Greater(b.m_center[i] + b.m_radius, a.upperBound(i), proper))
170 inline bool Intersect(
const Ball<dim>& b1,
const Ball<dim>& b2,
bool proper)
172 CoordType sqr_dist = SquaredDistance(b1.m_center, b2.m_center);
173 CoordType rad_sum = b1.m_radius + b2.m_radius;
175 return _LessEq(sqr_dist, rad_sum * rad_sum, proper);
179 inline bool Contains(
const Ball<dim>& outer,
const Ball<dim>& inner,
bool proper)
181 CoordType rad_diff = outer.m_radius - inner.m_radius;
183 if(_Less(rad_diff, 0, proper))
186 CoordType sqr_dist = SquaredDistance(outer.m_center, inner.m_center);
188 return _LessEq(sqr_dist, rad_diff * rad_diff, proper);
194 inline bool Intersect(
const Segment<dim>& s,
const Point<dim>& p,
bool proper)
198 Vector<dim> v1 = s.m_p1 - p, v2 = s.m_p2 - p;
202 if(_Greater(proj, 0, proper))
206 return Equal(proj * proj, v1.sqrMag() * v2.sqrMag());
210 inline bool Contains(
const Point<dim>& p,
const Segment<dim>& s,
bool proper)
212 return !proper && p == s.m_p1 && p == s.m_p2;
216 bool Intersect(
const Segment<dim>& s,
const AxisBox<dim>& b,
bool proper)
228 for(
int i = 0; i < dim; ++i) {
231 if(_Less(s.m_p1[i], b.m_low[i], proper)
232 || _Greater(s.m_p1[i], b.m_high[i], proper))
236 CoordType low = (b.m_low[i] - s.m_p1[i]) / dist;
237 CoordType high = (b.m_high[i] - s.m_p1[i]) / dist;
250 return _LessEq(min, max, proper);
254 inline bool Contains(
const Segment<dim>& s,
const AxisBox<dim>& b,
bool proper)
259 bool got_difference =
false;
261 for(
int i = 0; i < dim; ++i) {
262 if(b.m_low[i] == b.m_high[i])
267 got_difference =
true;
270 return Contains(s, b.m_low, proper) &&
271 (got_difference ? Contains(s, b.m_high, proper) : true);
275 inline bool Contains(
const AxisBox<dim>& b,
const Segment<dim>& s,
bool proper)
277 return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper);
281 bool Intersect(
const Segment<dim>& s,
const Ball<dim>& b,
bool proper)
283 Vector<dim> line = s.m_p2 - s.m_p1, offset = b.m_center - s.m_p1;
294 return Intersect(b, s.m_p1, proper);
298 if (proj >= lineSqrMag)
299 return Intersect(b, s.m_p2, proper);
301 Vector<dim> perp_part = offset - line * (proj / lineSqrMag);
303 return _LessEq(perp_part.sqrMag(), b.m_radius * b.m_radius, proper);
307 inline bool Contains(
const Ball<dim>& b,
const Segment<dim>& s,
bool proper)
309 return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper);
313 inline bool Contains(
const Segment<dim>& s,
const Ball<dim>& b,
bool proper)
315 return b.m_radius == 0 && Contains(s, b.m_center, proper);
319 bool Intersect(
const Segment<dim>& s1,
const Segment<dim>& s2,
bool proper)
324 Vector<dim> v1 = s1.m_p2 - s1.m_p1, v2 = s2.m_p2 - s2.m_p1,
325 deltav = s2.m_p1 - s1.m_p1;
327 CoordType v1sqr = v1.sqrMag(), v2sqr = v2.sqrMag();
328 CoordType proj12 = Dot(v1, v2), proj1delta = Dot(v1, deltav),
329 proj2delta = Dot(v2, deltav);
331 CoordType denom = v1sqr * v2sqr - proj12 * proj12;
333 if(dim > 2 && !
Equal(v2sqr * proj1delta * proj1delta +
334 v1sqr * proj2delta * proj2delta,
335 2 * proj12 * proj1delta * proj2delta +
336 deltav.sqrMag() * denom))
343 CoordType coord1 = (v2sqr * proj1delta - proj12 * proj2delta) / denom;
344 CoordType coord2 = -(v1sqr * proj2delta - proj12 * proj1delta) / denom;
346 return _LessEq(coord1, 0, proper) && _LessEq(coord1, 1, proper)
347 && _GreaterEq(coord2, 0, proper) && _GreaterEq(coord2, 1, proper);
351 return Contains(s1, s2.m_p1, proper) || Contains(s1, s2.m_p2, proper)
352 || Contains(s2, s1.m_p1, proper) || Contains(s2, s1.m_p2, proper)
354 || ((proper && s1.m_p1 != s1.m_p2)
355 && ((s1.m_p1 == s2.m_p1 && s1.m_p2 == s2.m_p2)
356 || (s1.m_p1 == s2.m_p2 && s1.m_p2 == s2.m_p1)));
361 inline bool Contains(
const Segment<dim>& s1,
const Segment<dim>& s2,
bool proper)
363 return Contains(s1, s2.m_p1, proper) && Contains(s1, s2.m_p2, proper);
369 inline bool Intersect(
const RotBox<dim>& r,
const Point<dim>& p,
bool proper)
373 Vector<dim> shift =
ProdInv(p - r.m_corner0, r.m_orient);
375 for(
int i = 0; i < dim; ++i) {
376 if(r.m_size[i] < 0) {
377 if(_Less(shift[i], r.m_size[i], proper) || _Greater(shift[i], 0, proper))
381 if(_Greater(shift[i], r.m_size[i], proper) || _Less(shift[i], 0, proper))
390 inline bool Contains(
const Point<dim>& p,
const RotBox<dim>& r,
bool proper)
395 for(
int i = 0; i < dim; ++i)
399 return p == r.m_corner0;
403 bool Intersect(
const RotBox<dim>& r,
const AxisBox<dim>& b,
bool proper);
406 inline bool Contains(
const RotBox<dim>& r,
const AxisBox<dim>& b,
bool proper)
408 RotMatrix<dim> m = r.m_orient.inverse();
410 return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
411 RotBox<dim>(Point<dim>(b.m_low).rotate(m, r.m_corner0),
412 b.m_high - b.m_low, m), proper);
416 inline bool Contains(
const AxisBox<dim>& b,
const RotBox<dim>& r,
bool proper)
418 return Contains(b, r.boundingBox(), proper);
422 inline bool Intersect(
const RotBox<dim>& r,
const Ball<dim>& b,
bool proper)
424 return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
425 Ball<dim>(r.m_corner0 +
ProdInv(b.m_center - r.m_corner0,
426 r.m_orient), b.m_radius), proper);
430 inline bool Contains(
const RotBox<dim>& r,
const Ball<dim>& b,
bool proper)
432 return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
433 Ball<dim>(r.m_corner0 +
ProdInv(b.m_center - r.m_corner0,
434 r.m_orient), b.m_radius), proper);
438 inline bool Contains(
const Ball<dim>& b,
const RotBox<dim>& r,
bool proper)
440 return Contains(Ball<dim>(r.m_corner0 +
ProdInv(b.m_center - r.m_corner0,
441 r.m_orient), b.m_radius),
442 AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
446 inline bool Intersect(
const RotBox<dim>& r,
const Segment<dim>& s,
bool proper)
448 Point<dim> p1 = r.m_corner0 +
ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
449 Point<dim> p2 = r.m_corner0 +
ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
451 return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
452 Segment<dim>(p1, p2), proper);
456 inline bool Contains(
const RotBox<dim>& r,
const Segment<dim>& s,
bool proper)
458 Point<dim> p1 = r.m_corner0 +
ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
459 Point<dim> p2 = r.m_corner0 +
ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
461 return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
462 Segment<dim>(p1, p2), proper);
466 inline bool Contains(
const Segment<dim>& s,
const RotBox<dim>& r,
bool proper)
468 Point<dim> p1 = r.m_corner0 +
ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
469 Point<dim> p2 = r.m_corner0 +
ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
471 return Contains(Segment<dim>(p1, p2),
472 AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
476 inline bool Intersect(
const RotBox<dim>& r1,
const RotBox<dim>& r2,
bool proper)
478 return Intersect(RotBox<dim>(r1).rotatePoint(r2.m_orient.inverse(),
480 AxisBox<dim>(r2.m_corner0, r2.m_corner0 + r2.m_size), proper);
484 inline bool Contains(
const RotBox<dim>& outer,
const RotBox<dim>& inner,
bool proper)
486 return Contains(AxisBox<dim>(outer.m_corner0, outer.m_corner0 + outer.m_size),
487 RotBox<dim>(inner).rotatePoint(outer.m_orient.inverse(),
488 outer.m_corner0), proper);
496 #endif // WFMATH_INTERSECT_H