wfmath  1.0.3
A math library for the Worldforge system.
intersect.cpp
1 // intersect.cpp (Backends to 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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 // Author: Ron Steinke
25 
26 #include "intersect.h"
27 
28 #include <cmath>
29 
30 #include <cassert>
31 
32 namespace WFMath {
33 
34 
35 // The 2d implementation was inspired as a simplification of the 3d.
36 // It used the fact that two not-similarly-oriented rectangles a and b
37 // intersect each other if and only if a's bounding box in b's coordinate
38 // system intersects b, and vice versa.
39 //
40 // To see this, it is only necessary to consider the bounding box intersection
41 // => intersection side of the assertion, since if the rectangles intersect,
42 // clearly their bounding boxes will as well. Let B(a) be the bounding box of
43 // a in b's coordinate system, and A(b) be the bounding box of b in a's coordinate
44 // system. Let the symbol ^ denote intersection. The thing we need to prove is:
45 //
46 // a ^ A(b) && b ^ B(a) => a ^ b
47 //
48 // I will discuss the equivalent statement
49 //
50 // a ^ A(b) && !(a ^ b) => !(b ^ B(a))
51 //
52 // If a intersects b's bounding box, but does not intersect b,
53 // the intersection of a and A(b) is a rectangle which lies in
54 // one of the four corners of the region A(b) - b (the set of all
55 // points which lie in the bounding box, but not in b). Without
56 // loss of generality, let a ^ A(b) lie in the lower left corner
57 // of A(b) - b. This is a triangular region, two of whose sides
58 // are parts of edges of A(b), the third being a side of b.
59 // Construct a line parallel to the side of b, passing between
60 // b and a ^ A(b). This line never intersects b, since b is a rectangle.
61 // It also never intersects a, since it passes above and to the
62 // right of the upper right corner of a. It also never intersects
63 // B(a), since the upper right side of B(a) is parallel to the
64 // line, but passes through the upper right corner of a. Thus,
65 // this line separates b from B(a), and they do not intersect. QED.
66 
67 template<>
68 bool Intersect<2>(const RotBox<2>& r, const AxisBox<2>& b, bool proper)
69 {
70  const AxisBox<2> b2 = r.boundingBox();
71  if(!Intersect(b2, b, proper))
72  return false;
73 
74  RotMatrix<2> m = r.m_orient.inverse();
75 
76  const AxisBox<2> b3 = RotBox<2>(Point<2>(b.m_low).rotate(m, r.m_corner0),
77  b.m_high - b.m_low, m).boundingBox();
78  const AxisBox<2> b4(r.m_corner0, r.m_corner0 + r.m_size);
79  return Intersect(b3, b4, proper);
80 }
81 
82 // The 3d implementation is based on the following theorem:
83 //
84 // Theorem:
85 //
86 // Two convex polyhedra do not intersect if and only if there exists a separating
87 // plane which is either parallel to the face of one polyhedron or which is
88 // parallel to at least one edge of each polyhedron.
89 //
90 // Found this in the abstract to the paper:
91 //
92 // Surface-to-surface intersection based on triangular parameter domain subdivision
93 // Ernst Huber
94 // Institute of Computer Graphics
95 // Vienna University of Technology
96 // A-1040 Vienna, Karlsplatz 13/186/1, Austria
97 //
98 // online postscript version of the abstract (where I got this from):
99 //
100 // http://www.cs.ubc.ca/conferences/CCCG/elec_proc/c48.ps.gz
101 //
102 // The paper gives a reference for the theorem (probably the one to really look at
103 // for proof/details):
104 //
105 // S. Gottschalk
106 // Separating Axis Theorem
107 // Technical Report TR96-024
108 // Department of Computer Science
109 // UNC Chapel Hill
110 // 1996
111 
112 template<>
113 bool Intersect<3>(const RotBox<3>& r, const AxisBox<3>& b, bool proper)
114 {
115  // Checking intersection of each with the bounding box of
116  // the other in the coordinate system of the first will take care
117  // of the "plane parallel to face" case
118 
119  const AxisBox<3> b2 = r.boundingBox();
120  if(!Intersect(b2, b, proper))
121  return false;
122 
123  RotMatrix<3> minv = r.m_orient.inverse();
124  Vector<3> b_size = b.m_high - b.m_low;
125 
126  const AxisBox<3> b3 = RotBox<3>(Point<3>(b.m_low).rotate(minv, r.m_corner0),
127  b_size, minv).boundingBox();
128  const AxisBox<3> b4(r.m_corner0, r.m_corner0 + r.m_size);
129  if(!Intersect(b3, b4, proper))
130  return false;
131 
132  // Now for the "plane parallel to at least one edge of each" case
133 
134  Vector<3> sep = b.m_low - r.m_corner0;
135  const RotMatrix<3> &m = r.m_orient;
136 
137  // Generate normals to the 9 possible separating planes
138 
139  for(int i = 0; i < 3; ++i) {
140  // Generate edge vectors for the RotBox, ignore size, only care about direction
141 // Just access m_orient directly below instead of using r_vec
142 // Vector<3> r_vec = m.row(i);
143 
144  for(int j = 0; j < 3; ++j) {
145  Vector<3> axis;
146 
147  // Cross product, ignore size of b since we only care about direction
148 
149  switch(j) {
150  case 0:
151  axis[0] = 0;
152  axis[1] = -m.elem(i, 2);
153  axis[2] = m.elem(i, 1);
154  break;
155  case 1:
156  axis[0] = m.elem(i, 2);
157  axis[1] = 0;
158  axis[2] = -m.elem(i, 0);
159  break;
160  case 2:
161  axis[0] = -m.elem(i, 1);
162  axis[1] = m.elem(i, 0);
163  axis[2] = 0;
164  break;
165  default:
166  assert(false);
167  }
168 
170  // Parallel edges, this reduces to the 2d case above. We've already
171  // checked the bounding box intersections, so we know they intersect.
172  // We don't need to scale WFMATH_EPSILON, det(m_orient) = 1
173  // essentially takes care of that.
174  return true;
175  }
176 
177  // Project both boxes on this axis, check for separating plane.
178 
179  // We only need to project two axes per box, the one parallel
180  // to the plane doesn't contribute
181 
182  const int next[] = {1, 2, 0};
183  CoordType val;
184  CoordType b_low, b_high, r_low, r_high, dist;
185  int k;
186 
187  // AxisBox projection
188 
189  k = next[j];
190 
191  val = axis[k] * b_size[k];
192 
193  if(val > 0) {
194  b_high = val;
195  b_low = 0;
196  }
197  else {
198  b_low = val;
199  b_high = 0;
200  }
201 
202  k = next[k];
203 
204  val = axis[k] * b_size[k];
205 
206  if(val > 0)
207  b_high += val;
208  else
209  b_low += val;
210 
211  // RotBox projection
212 
213  k = next[i];
214 
215  val = Dot(m.row(k), axis) * r.m_size[k];
216 
217  if(val > 0) {
218  r_high = val;
219  r_low = 0;
220  }
221  else {
222  r_low = val;
223  r_high = 0;
224  }
225 
226  k = next[k];
227 
228  val = Dot(m.row(k), axis) * r.m_size[k];
229 
230  if(val > 0)
231  r_high += val;
232  else
233  r_low += val;
234 
235  // Distance between basepoints for boxes along this axis
236 
237  dist = Dot(sep, axis);
238 
239  if(_Greater(r_low - dist, b_high, proper)
240  || _Less(r_high - dist, b_low, proper))
241  return false;
242  }
243  }
244 
245  return true;
246 }
247 
248 // force a bunch of instantiations
249 
250 template bool Intersect<2>(const Point<2>&, const Point<2>&, bool);
251 template bool Intersect<3>(const Point<3>&, const Point<3>&, bool);
252 template bool Contains<2>(const Point<2>&, const Point<2>&, bool);
253 template bool Contains<3>(const Point<3>&, const Point<3>&, bool);
254 
255 template bool Intersect<Point<2>,AxisBox<2> >(const Point<2>&, const AxisBox<2>&, bool);
256 template bool Intersect<Point<3>,AxisBox<3> >(const Point<3>&, const AxisBox<3>&, bool);
257 template bool Contains<2>(const Point<2>&, const AxisBox<2>&, bool);
258 template bool Contains<3>(const Point<3>&, const AxisBox<3>&, bool);
259 template bool Intersect<2>(const AxisBox<2>&, const Point<2>&, bool);
260 template bool Intersect<3>(const AxisBox<3>&, const Point<3>&, bool);
261 template bool Contains<2>(const AxisBox<2>&, const Point<2>&, bool);
262 template bool Contains<3>(const AxisBox<3>&, const Point<3>&, bool);
263 
264 template bool Intersect<2>(const AxisBox<2>&, const AxisBox<2>&, bool);
265 template bool Intersect<3>(const AxisBox<3>&, const AxisBox<3>&, bool);
266 template bool Contains<2>(const AxisBox<2>&, const AxisBox<2>&, bool);
267 template bool Contains<3>(const AxisBox<3>&, const AxisBox<3>&, bool);
268 
269 template bool Intersect<Point<2>,Ball<2> >(const Point<2>&, const Ball<2>&, bool);
270 template bool Intersect<Point<3>,Ball<3> >(const Point<3>&, const Ball<3>&, bool);
271 template bool Contains<2>(const Point<2>&, const Ball<2>&, bool);
272 template bool Contains<3>(const Point<3>&, const Ball<3>&, bool);
273 template bool Intersect<2>(const Ball<2>&, const Point<2>&, bool);
274 template bool Intersect<3>(const Ball<3>&, const Point<3>&, bool);
275 template bool Contains<2>(const Ball<2>&, const Point<2>&, bool);
276 template bool Contains<3>(const Ball<3>&, const Point<3>&, bool);
277 
278 template bool Intersect<AxisBox<2>,Ball<2> >(const AxisBox<2>&, const Ball<2>&, bool);
279 template bool Intersect<AxisBox<3>,Ball<3> >(const AxisBox<3>&, const Ball<3>&, bool);
280 template bool Contains<2>(const AxisBox<2>&, const Ball<2>&, bool);
281 template bool Contains<3>(const AxisBox<3>&, const Ball<3>&, bool);
282 template bool Intersect<2>(const Ball<2>&, const AxisBox<2>&, bool);
283 template bool Intersect<3>(const Ball<3>&, const AxisBox<3>&, bool);
284 template bool Contains<2>(const Ball<2>&, const AxisBox<2>&, bool);
285 template bool Contains<3>(const Ball<3>&, const AxisBox<3>&, bool);
286 
287 template bool Intersect<2>(const Ball<2>&, const Ball<2>&, bool);
288 template bool Intersect<3>(const Ball<3>&, const Ball<3>&, bool);
289 template bool Contains<2>(const Ball<2>&, const Ball<2>&, bool);
290 template bool Contains<3>(const Ball<3>&, const Ball<3>&, bool);
291 
292 template bool Intersect<Point<2>,Segment<2> >(const Point<2>&, const Segment<2>&, bool);
293 template bool Intersect<Point<3>,Segment<3> >(const Point<3>&, const Segment<3>&, bool);
294 template bool Contains<2>(const Point<2>&, const Segment<2>&, bool);
295 template bool Contains<3>(const Point<3>&, const Segment<3>&, bool);
296 template bool Intersect<2>(const Segment<2>&, const Point<2>&, bool);
297 template bool Intersect<3>(const Segment<3>&, const Point<3>&, bool);
298 template bool Contains<2>(const Segment<2>&, const Point<2>&, bool);
299 template bool Contains<3>(const Segment<3>&, const Point<3>&, bool);
300 
301 template bool Intersect<AxisBox<2>,Segment<2> >(const AxisBox<2>&, const Segment<2>&, bool);
302 template bool Intersect<AxisBox<3>,Segment<3> >(const AxisBox<3>&, const Segment<3>&, bool);
303 template bool Contains<2>(const AxisBox<2>&, const Segment<2>&, bool);
304 template bool Contains<3>(const AxisBox<3>&, const Segment<3>&, bool);
305 template bool Intersect<2>(const Segment<2>&, const AxisBox<2>&, bool);
306 template bool Intersect<3>(const Segment<3>&, const AxisBox<3>&, bool);
307 template bool Contains<2>(const Segment<2>&, const AxisBox<2>&, bool);
308 template bool Contains<3>(const Segment<3>&, const AxisBox<3>&, bool);
309 
310 template bool Intersect<Ball<2>,Segment<2> >(const Ball<2>&, const Segment<2>&, bool);
311 template bool Intersect<Ball<3>,Segment<3> >(const Ball<3>&, const Segment<3>&, bool);
312 template bool Contains<2>(const Ball<2>&, const Segment<2>&, bool);
313 template bool Contains<3>(const Ball<3>&, const Segment<3>&, bool);
314 template bool Intersect<2>(const Segment<2>&, const Ball<2>&, bool);
315 template bool Intersect<3>(const Segment<3>&, const Ball<3>&, bool);
316 template bool Contains<2>(const Segment<2>&, const Ball<2>&, bool);
317 template bool Contains<3>(const Segment<3>&, const Ball<3>&, bool);
318 
319 template bool Intersect<2>(const Segment<2>&, const Segment<2>&, bool);
320 template bool Intersect<3>(const Segment<3>&, const Segment<3>&, bool);
321 template bool Contains<2>(const Segment<2>&, const Segment<2>&, bool);
322 template bool Contains<3>(const Segment<3>&, const Segment<3>&, bool);
323 
324 template bool Intersect<Point<2>,RotBox<2> >(const Point<2>&, const RotBox<2>&, bool);
325 template bool Intersect<Point<3>,RotBox<3> >(const Point<3>&, const RotBox<3>&, bool);
326 template bool Contains<2>(const Point<2>&, const RotBox<2>&, bool);
327 template bool Contains<3>(const Point<3>&, const RotBox<3>&, bool);
328 template bool Intersect<2>(const RotBox<2>&, const Point<2>&, bool);
329 template bool Intersect<3>(const RotBox<3>&, const Point<3>&, bool);
330 template bool Contains<2>(const RotBox<2>&, const Point<2>&, bool);
331 template bool Contains<3>(const RotBox<3>&, const Point<3>&, bool);
332 
333 template bool Intersect<AxisBox<2>,RotBox<2> >(const AxisBox<2>&, const RotBox<2>&, bool);
334 template bool Intersect<AxisBox<3>,RotBox<3> >(const AxisBox<3>&, const RotBox<3>&, bool);
335 template bool Contains<2>(const AxisBox<2>&, const RotBox<2>&, bool);
336 template bool Contains<3>(const AxisBox<3>&, const RotBox<3>&, bool);
337 template bool Contains<2>(const RotBox<2>&, const AxisBox<2>&, bool);
338 template bool Contains<3>(const RotBox<3>&, const AxisBox<3>&, bool);
339 
340 template bool Intersect<Ball<2>,RotBox<2> >(const Ball<2>&, const RotBox<2>&, bool);
341 template bool Intersect<Ball<3>,RotBox<3> >(const Ball<3>&, const RotBox<3>&, bool);
342 template bool Contains<2>(const Ball<2>&, const RotBox<2>&, bool);
343 template bool Contains<3>(const Ball<3>&, const RotBox<3>&, bool);
344 template bool Intersect<2>(const RotBox<2>&, const Ball<2>&, bool);
345 template bool Intersect<3>(const RotBox<3>&, const Ball<3>&, bool);
346 template bool Contains<2>(const RotBox<2>&, const Ball<2>&, bool);
347 template bool Contains<3>(const RotBox<3>&, const Ball<3>&, bool);
348 
349 template bool Intersect<Segment<2>,RotBox<2> >(const Segment<2>&, const RotBox<2>&, bool);
350 template bool Intersect<Segment<3>,RotBox<3> >(const Segment<3>&, const RotBox<3>&, bool);
351 template bool Contains<2>(const Segment<2>&, const RotBox<2>&, bool);
352 template bool Contains<3>(const Segment<3>&, const RotBox<3>&, bool);
353 template bool Intersect<2>(const RotBox<2>&, const Segment<2>&, bool);
354 template bool Intersect<3>(const RotBox<3>&, const Segment<3>&, bool);
355 template bool Contains<2>(const RotBox<2>&, const Segment<2>&, bool);
356 template bool Contains<3>(const RotBox<3>&, const Segment<3>&, bool);
357 
358 template bool Intersect<2>(const RotBox<2>&, const RotBox<2>&, bool);
359 template bool Intersect<3>(const RotBox<3>&, const RotBox<3>&, bool);
360 template bool Contains<2>(const RotBox<2>&, const RotBox<2>&, bool);
361 template bool Contains<3>(const RotBox<3>&, const RotBox<3>&, bool);
362 
363 
364 
365 }
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
static FloatType epsilon()
This is the attempted precision of the library.