WFMath 1.0.2
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
38namespace 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
44template<class S1, class S2>
45inline bool Intersect(const S1& s1, const S2& s2, bool proper)
46{
47 return Intersect(s2, s1, proper);
48}
49
50// Point<>
51
52template<int dim>
53inline bool Intersect(const Point<dim>& p1, const Point<dim>& p2, bool proper)
54{
55 return !proper && p1 == p2;
56}
57
58template<int dim, class S>
59inline bool Contains(const S& s, const Point<dim>& p, bool proper)
60{
61 return Intersect(p, s, proper);
62}
63
64template<int dim>
65inline bool Contains(const Point<dim>& p1, const Point<dim>& p2, bool proper)
66{
67 return !proper && p1 == p2;
68}
69
70// AxisBox<>
71
72template<int dim>
73inline 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
82template<int dim>
83inline 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
88template<int dim>
89inline 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
99template<int dim>
100inline 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
112template<int dim>
113inline 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
119template<int dim>
120inline 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
125template<int dim>
126inline 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
144template<int dim>
145inline 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
158template<int dim>
159inline 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
169template<int dim>
170inline 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
178template<int dim>
179inline 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
193template<int dim>
194inline 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
209template<int dim>
210inline 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
215template<int dim>
216bool 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
253template<int dim>
254inline 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
274template<int dim>
275inline 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
280template<int dim>
281bool 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
306template<int dim>
307inline 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
312template<int dim>
313inline 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
318template<int dim>
319bool 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
360template<int dim>
361inline 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
368template<int dim>
369inline 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
389template<int dim>
390inline 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
402template<int dim>
403bool Intersect(const RotBox<dim>& r, const AxisBox<dim>& b, bool proper);
404
405template<int dim>
406inline 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
415template<int dim>
416inline bool Contains(const AxisBox<dim>& b, const RotBox<dim>& r, bool proper)
417{
418 return Contains(b, r.boundingBox(), proper);
419}
420
421template<int dim>
422inline 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
429template<int dim>
430inline 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
437template<int dim>
438inline 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
445template<int dim>
446inline 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
455template<int dim>
456inline 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
465template<int dim>
466inline 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
475template<int dim>
476inline 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
483template<int dim>
484inline 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
Generic library namespace.
Definition atlasconv.h:45
bool Equal(const C &c1, const C &c2, CoordType epsilon=numeric_constants< CoordType >::epsilon())
Test for equality up to precision epsilon.
Definition const.h:158
float CoordType
Basic floating point type.
Definition const.h:140
RotMatrix< dim > ProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2^-1
Definition rotmatrix_funcs.h:111