WFMath  1.0.1
intersect.h
1 // intersect.h (Shape intersection functions)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2002 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 
24 #ifndef WFMATH_INTERSECT_H
25 #define WFMATH_INTERSECT_H
26 
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>
35 
36 #include <cmath>
37 
38 namespace WFMath {
39 
40 // Get the reversed order intersect functions (is this safe? FIXME)
41 // No it's not. In the case of an unknown intersection we end up in
42 // a stack crash loop.
43 
44 template<class S1, class S2>
45 inline bool Intersect(const S1& s1, const S2& s2, bool proper)
46 {
47  return Intersect(s2, s1, proper);
48 }
49 
50 // Point<>
51 
52 template<int dim>
53 inline bool Intersect(const Point<dim>& p1, const Point<dim>& p2, bool proper)
54 {
55  return !proper && p1 == p2;
56 }
57 
58 template<int dim, class S>
59 inline bool Contains(const S& s, const Point<dim>& p, bool proper)
60 {
61  return Intersect(p, s, proper);
62 }
63 
64 template<int dim>
65 inline bool Contains(const Point<dim>& p1, const Point<dim>& p2, bool proper)
66 {
67  return !proper && p1 == p2;
68 }
69 
70 // AxisBox<>
71 
72 template<int dim>
73 inline bool Intersect(const AxisBox<dim>& b, const Point<dim>& p, bool proper)
74 {
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))
77  return false;
78 
79  return true;
80 }
81 
82 template<int dim>
83 inline bool Contains(const Point<dim>& p, const AxisBox<dim>& b, bool proper)
84 {
85  return !proper && p == b.m_low && p == b.m_high;
86 }
87 
88 template<int dim>
89 inline bool Intersect(const AxisBox<dim>& b1, const AxisBox<dim>& b2, bool proper)
90 {
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))
94  return false;
95 
96  return true;
97 }
98 
99 template<int dim>
100 inline bool Contains(const AxisBox<dim>& outer, const AxisBox<dim>& inner, bool proper)
101 {
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))
105  return false;
106 
107  return true;
108 }
109 
110 // Ball<>
111 
112 template<int dim>
113 inline bool Intersect(const Ball<dim>& b, const Point<dim>& p, bool proper)
114 {
115  return _LessEq(SquaredDistance(b.m_center, p), b.m_radius * b.m_radius
116  * (1 + numeric_constants<CoordType>::epsilon()), proper);
117 }
118 
119 template<int dim>
120 inline bool Contains(const Point<dim>& p, const Ball<dim>& b, bool proper)
121 {
122  return !proper && b.m_radius == 0 && p == b.m_center;
123 }
124 
125 template<int dim>
126 inline bool Intersect(const Ball<dim>& b, const AxisBox<dim>& a, bool proper)
127 {
128  CoordType dist = 0;
129 
130  for(int i = 0; i < dim; ++i) {
131  CoordType dist_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];
136  else
137  continue;
138  dist+= dist_i * dist_i;
139  }
140 
141  return _LessEq(dist, b.m_radius * b.m_radius, proper);
142 }
143 
144 template<int dim>
145 inline bool Contains(const Ball<dim>& b, const AxisBox<dim>& a, bool proper)
146 {
147  CoordType sqr_dist = 0;
148 
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;
153  }
154 
155  return _LessEq(sqr_dist, b.m_radius * b.m_radius * (1 + numeric_constants<CoordType>::epsilon()), proper);
156 }
157 
158 template<int dim>
159 inline bool Contains(const AxisBox<dim>& a, const Ball<dim>& b, bool proper)
160 {
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))
164  return false;
165 
166  return true;
167 }
168 
169 template<int dim>
170 inline bool Intersect(const Ball<dim>& b1, const Ball<dim>& b2, bool proper)
171 {
172  CoordType sqr_dist = SquaredDistance(b1.m_center, b2.m_center);
173  CoordType rad_sum = b1.m_radius + b2.m_radius;
174 
175  return _LessEq(sqr_dist, rad_sum * rad_sum, proper);
176 }
177 
178 template<int dim>
179 inline bool Contains(const Ball<dim>& outer, const Ball<dim>& inner, bool proper)
180 {
181  CoordType rad_diff = outer.m_radius - inner.m_radius;
182 
183  if(_Less(rad_diff, 0, proper))
184  return false;
185 
186  CoordType sqr_dist = SquaredDistance(outer.m_center, inner.m_center);
187 
188  return _LessEq(sqr_dist, rad_diff * rad_diff, proper);
189 }
190 
191 // Segment<>
192 
193 template<int dim>
194 inline bool Intersect(const Segment<dim>& s, const Point<dim>& p, bool proper)
195 {
196  // This is only true if p lies on the line between m_p1 and m_p2
197 
198  Vector<dim> v1 = s.m_p1 - p, v2 = s.m_p2 - p;
199 
200  CoordType proj = Dot(v1, v2);
201 
202  if(_Greater(proj, 0, proper)) // p is on the same side of both ends, not between them
203  return false;
204 
205  // Check for colinearity
206  return Equal(proj * proj, v1.sqrMag() * v2.sqrMag());
207 }
208 
209 template<int dim>
210 inline bool Contains(const Point<dim>& p, const Segment<dim>& s, bool proper)
211 {
212  return !proper && p == s.m_p1 && p == s.m_p2;
213 }
214 
215 template<int dim>
216 bool Intersect(const Segment<dim>& s, const AxisBox<dim>& b, bool proper)
217 {
218  // Use parametric coordinates on the line, where 0 is the location
219  // of m_p1 and 1 is the location of m_p2
220 
221  // Find the parametric coordinates of the portion of the line
222  // which lies betweens b.lowerBound(i) and b.upperBound(i) for
223  // each i. Find the intersection of those segments and the
224  // segment (0, 1), and check if it's nonzero.
225 
226  CoordType min = 0, max = 1;
227 
228  for(int i = 0; i < dim; ++i) {
229  CoordType dist = s.m_p2[i] - s.m_p1[i];
230  if(dist == 0) {
231  if(_Less(s.m_p1[i], b.m_low[i], proper)
232  || _Greater(s.m_p1[i], b.m_high[i], proper))
233  return false;
234  }
235  else {
236  CoordType low = (b.m_low[i] - s.m_p1[i]) / dist;
237  CoordType high = (b.m_high[i] - s.m_p1[i]) / dist;
238  if(low > high) {
239  CoordType tmp = high;
240  high = low;
241  low = tmp;
242  }
243  if(low > min)
244  min = low;
245  if(high < max)
246  max = high;
247  }
248  }
249 
250  return _LessEq(min, max, proper);
251 }
252 
253 template<int dim>
254 inline bool Contains(const Segment<dim>& s, const AxisBox<dim>& b, bool proper)
255 {
256  // This is only possible for zero width or zero height box,
257  // in which case we check for containment of the endpoints.
258 
259  bool got_difference = false;
260 
261  for(int i = 0; i < dim; ++i) {
262  if(b.m_low[i] == b.m_high[i])
263  continue;
264  if(got_difference)
265  return false;
266  else // It's okay to be different on one axis
267  got_difference = true;
268  }
269 
270  return Contains(s, b.m_low, proper) &&
271  (got_difference ? Contains(s, b.m_high, proper) : true);
272 }
273 
274 template<int dim>
275 inline bool Contains(const AxisBox<dim>& b, const Segment<dim>& s, bool proper)
276 {
277  return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper);
278 }
279 
280 template<int dim>
281 bool Intersect(const Segment<dim>& s, const Ball<dim>& b, bool proper)
282 {
283  Vector<dim> line = s.m_p2 - s.m_p1, offset = b.m_center - s.m_p1;
284 
285  // First, see if the closest point on the line to the center of
286  // the ball lies inside the segment
287 
288  CoordType proj = Dot(line, offset);
289 
290  // If the nearest point on the line is outside the segment,
291  // intersection reduces to checking the nearest endpoint.
292 
293  if(proj <= 0)
294  return Intersect(b, s.m_p1, proper);
295 
296  CoordType lineSqrMag = line.sqrMag();
297 
298  if (proj >= lineSqrMag)
299  return Intersect(b, s.m_p2, proper);
300 
301  Vector<dim> perp_part = offset - line * (proj / lineSqrMag);
302 
303  return _LessEq(perp_part.sqrMag(), b.m_radius * b.m_radius, proper);
304 }
305 
306 template<int dim>
307 inline bool Contains(const Ball<dim>& b, const Segment<dim>& s, bool proper)
308 {
309  return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper);
310 }
311 
312 template<int dim>
313 inline bool Contains(const Segment<dim>& s, const Ball<dim>& b, bool proper)
314 {
315  return b.m_radius == 0 && Contains(s, b.m_center, proper);
316 }
317 
318 template<int dim>
319 bool Intersect(const Segment<dim>& s1, const Segment<dim>& s2, bool proper)
320 {
321  // Check that the lines that contain the segments intersect, and then check
322  // that the intersection point lies within the segments
323 
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;
326 
327  CoordType v1sqr = v1.sqrMag(), v2sqr = v2.sqrMag();
328  CoordType proj12 = Dot(v1, v2), proj1delta = Dot(v1, deltav),
329  proj2delta = Dot(v2, deltav);
330 
331  CoordType denom = v1sqr * v2sqr - proj12 * proj12;
332 
333  if(dim > 2 && !Equal(v2sqr * proj1delta * proj1delta +
334  v1sqr * proj2delta * proj2delta,
335  2 * proj12 * proj1delta * proj2delta +
336  deltav.sqrMag() * denom))
337  return false; // Skew lines; don't intersect
338 
339  if(denom > 0) {
340  // Find the location of the intersection point in parametric coordinates,
341  // where one end of the segment is at zero and the other at one
342 
343  CoordType coord1 = (v2sqr * proj1delta - proj12 * proj2delta) / denom;
344  CoordType coord2 = -(v1sqr * proj2delta - proj12 * proj1delta) / denom;
345 
346  return _LessEq(coord1, 0, proper) && _LessEq(coord1, 1, proper)
347  && _GreaterEq(coord2, 0, proper) && _GreaterEq(coord2, 1, proper);
348  }
349  else {
350  // Parallel segments, see if one contains an endpoint of the other
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)
353  // Degenerate case (identical segments), nonzero length
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)));
357  }
358 }
359 
360 template<int dim>
361 inline bool Contains(const Segment<dim>& s1, const Segment<dim>& s2, bool proper)
362 {
363  return Contains(s1, s2.m_p1, proper) && Contains(s1, s2.m_p2, proper);
364 }
365 
366 // RotBox<>
367 
368 template<int dim>
369 inline bool Intersect(const RotBox<dim>& r, const Point<dim>& p, bool proper)
370 {
371  // Rotate the point into the internal coordinate system of the box
372 
373  Vector<dim> shift = ProdInv(p - r.m_corner0, r.m_orient);
374 
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))
378  return false;
379  }
380  else {
381  if(_Greater(shift[i], r.m_size[i], proper) || _Less(shift[i], 0, proper))
382  return false;
383  }
384  }
385 
386  return true;
387 }
388 
389 template<int dim>
390 inline bool Contains(const Point<dim>& p, const RotBox<dim>& r, bool proper)
391 {
392  if(proper)
393  return false;
394 
395  for(int i = 0; i < dim; ++i)
396  if(r.m_size[i] != 0)
397  return false;
398 
399  return p == r.m_corner0;
400 }
401 
402 template<int dim>
403 bool Intersect(const RotBox<dim>& r, const AxisBox<dim>& b, bool proper);
404 
405 template<int dim>
406 inline bool Contains(const RotBox<dim>& r, const AxisBox<dim>& b, bool proper)
407 {
408  RotMatrix<dim> m = r.m_orient.inverse();
409 
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);
413 }
414 
415 template<int dim>
416 inline bool Contains(const AxisBox<dim>& b, const RotBox<dim>& r, bool proper)
417 {
418  return Contains(b, r.boundingBox(), proper);
419 }
420 
421 template<int dim>
422 inline bool Intersect(const RotBox<dim>& r, const Ball<dim>& b, bool proper)
423 {
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);
427 }
428 
429 template<int dim>
430 inline bool Contains(const RotBox<dim>& r, const Ball<dim>& b, bool proper)
431 {
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);
435 }
436 
437 template<int dim>
438 inline bool Contains(const Ball<dim>& b, const RotBox<dim>& r, bool proper)
439 {
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);
443 }
444 
445 template<int dim>
446 inline bool Intersect(const RotBox<dim>& r, const Segment<dim>& s, bool proper)
447 {
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);
450 
451  return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
452  Segment<dim>(p1, p2), proper);
453 }
454 
455 template<int dim>
456 inline bool Contains(const RotBox<dim>& r, const Segment<dim>& s, bool proper)
457 {
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);
460 
461  return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
462  Segment<dim>(p1, p2), proper);
463 }
464 
465 template<int dim>
466 inline bool Contains(const Segment<dim>& s, const RotBox<dim>& r, bool proper)
467 {
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);
470 
471  return Contains(Segment<dim>(p1, p2),
472  AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
473 }
474 
475 template<int dim>
476 inline bool Intersect(const RotBox<dim>& r1, const RotBox<dim>& r2, bool proper)
477 {
478  return Intersect(RotBox<dim>(r1).rotatePoint(r2.m_orient.inverse(),
479  r2.m_corner0),
480  AxisBox<dim>(r2.m_corner0, r2.m_corner0 + r2.m_size), proper);
481 }
482 
483 template<int dim>
484 inline bool Contains(const RotBox<dim>& outer, const RotBox<dim>& inner, bool proper)
485 {
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);
489 }
490 
491 // Polygon<> intersection functions are in polygon_funcs.h, to avoid
492 // unnecessary inclusion of <vector>
493 
494 } // namespace WFMath
495 
496 #endif // WFMATH_INTERSECT_H