Main Page | Namespace List | Class List | File List | Namespace Members | Class Members

intersect.h

00001 // intersect.h (Shape intersection functions) 00002 // 00003 // The WorldForge Project 00004 // Copyright (C) 2002 The WorldForge Project 00005 // 00006 // This program is free software; you can redistribute it and/or modify 00007 // it under the terms of the GNU General Public License as published by 00008 // the Free Software Foundation; either version 2 of the License, or 00009 // (at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU General Public License 00017 // along with this program; if not, write to the Free Software 00018 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00019 // 00020 // For information about WorldForge and its authors, please contact 00021 // the Worldforge Web Site at http://www.worldforge.org. 00022 // 00023 00024 #ifndef WFMATH_INTERSECT_H 00025 #define WFMATH_INTERSECT_H 00026 00027 #include <wfmath/vector.h> 00028 #include <wfmath/point.h> 00029 #include <wfmath/const.h> 00030 #include <wfmath/intersect_decls.h> 00031 #include <wfmath/axisbox.h> 00032 #include <wfmath/ball.h> 00033 #include <wfmath/segment.h> 00034 #include <wfmath/rotbox.h> 00035 00036 namespace WFMath { 00037 00038 // Get the reversed order intersect functions (is this safe? FIXME) 00039 00040 template<class S1, class S2> 00041 inline bool Intersect(const S1& s1, const S2& s2, bool proper) 00042 { 00043 return Intersect(s2, s1, proper); 00044 } 00045 00046 // Point<> 00047 00048 template<const int dim> 00049 inline bool Intersect(const Point<dim>& p1, const Point<dim>& p2, bool proper) 00050 { 00051 return !proper && p1 == p2; 00052 } 00053 00054 template<const int dim, class S> 00055 inline bool Contains(const S& s, const Point<dim>& p, bool proper) 00056 { 00057 return Intersect(p, s, proper); 00058 } 00059 00060 template<const int dim> 00061 inline bool Contains(const Point<dim>& p1, const Point<dim>& p2, bool proper) 00062 { 00063 return !proper && p1 == p2; 00064 } 00065 00066 // AxisBox<> 00067 00068 template<const int dim> 00069 inline bool Intersect(const AxisBox<dim>& b, const Point<dim>& p, bool proper) 00070 { 00071 for(int i = 0; i < dim; ++i) 00072 if(_Greater(b.m_low[i], p[i], proper) || _Less(b.m_high[i], p[i], proper)) 00073 return false; 00074 00075 return true; 00076 } 00077 00078 template<const int dim> 00079 inline bool Contains(const Point<dim>& p, const AxisBox<dim>& b, bool proper) 00080 { 00081 return !proper && p == b.m_low && p == b.m_high; 00082 } 00083 00084 template<const int dim> 00085 inline bool Intersect(const AxisBox<dim>& b1, const AxisBox<dim>& b2, bool proper) 00086 { 00087 for(int i = 0; i < dim; ++i) 00088 if(_Greater(b1.m_low[i], b2.m_high[i], proper) 00089 || _Less(b1.m_high[i], b2.m_low[i], proper)) 00090 return false; 00091 00092 return true; 00093 } 00094 00095 template<const int dim> 00096 inline bool Contains(const AxisBox<dim>& outer, const AxisBox<dim>& inner, bool proper) 00097 { 00098 for(int i = 0; i < dim; ++i) 00099 if(_Less(inner.m_low[i], outer.m_low[i], proper) 00100 || _Greater(inner.m_high[i], outer.m_high[i], proper)) 00101 return false; 00102 00103 return true; 00104 } 00105 00106 // Ball<> 00107 00108 template<const int dim> 00109 inline bool Intersect(const Ball<dim>& b, const Point<dim>& p, bool proper) 00110 { 00111 return _LessEq(SquaredDistance(b.m_center, p), b.m_radius * b.m_radius 00112 * (1 + WFMATH_EPSILON), proper); 00113 } 00114 00115 template<const int dim> 00116 inline bool Contains(const Point<dim>& p, const Ball<dim>& b, bool proper) 00117 { 00118 return !proper && b.m_radius == 0 && p == b.m_center; 00119 } 00120 00121 template<const int dim> 00122 inline bool Intersect(const Ball<dim>& b, const AxisBox<dim>& a, bool proper) 00123 { 00124 CoordType dist = 0; 00125 00126 for(int i = 0; i < dim; ++i) { 00127 CoordType dist_i; 00128 if(b.m_center[i] < a.m_low[i]) 00129 dist_i = b.m_center[i] - a.m_low[i]; 00130 else if(b.m_center[i] > a.m_high[i]) 00131 dist_i = b.m_center[i] - a.m_high[i]; 00132 else 00133 continue; 00134 dist+= dist_i * dist_i; 00135 } 00136 00137 return _LessEq(dist, b.m_radius * b.m_radius, proper); 00138 } 00139 00140 template<const int dim> 00141 inline bool Contains(const Ball<dim>& b, const AxisBox<dim>& a, bool proper) 00142 { 00143 CoordType sqr_dist = 0; 00144 00145 for(int i = 0; i < dim; ++i) { 00146 CoordType furthest = FloatMax(fabs(b.m_center[i] - a.m_low[i]), 00147 fabs(b.m_center[i] - a.m_high[i])); 00148 sqr_dist += furthest * furthest; 00149 } 00150 00151 return _LessEq(sqr_dist, b.m_radius * b.m_radius * (1 + WFMATH_EPSILON), proper); 00152 } 00153 00154 template<const int dim> 00155 inline bool Contains(const AxisBox<dim>& a, const Ball<dim>& b, bool proper) 00156 { 00157 for(int i = 0; i < dim; ++i) 00158 if(_Less(b.m_center[i] - b.m_radius, a.lowerBound(i), proper) 00159 || _Greater(b.m_center[i] + b.m_radius, a.upperBound(i), proper)) 00160 return false; 00161 00162 return true; 00163 } 00164 00165 template<const int dim> 00166 inline bool Intersect(const Ball<dim>& b1, const Ball<dim>& b2, bool proper) 00167 { 00168 CoordType sqr_dist = SquaredDistance(b1.m_center, b2.m_center); 00169 CoordType rad_sum = b1.m_radius + b2.m_radius; 00170 00171 return _LessEq(sqr_dist, rad_sum * rad_sum, proper); 00172 } 00173 00174 template<const int dim> 00175 inline bool Contains(const Ball<dim>& outer, const Ball<dim>& inner, bool proper) 00176 { 00177 CoordType rad_diff = outer.m_radius - inner.m_radius; 00178 00179 if(_Less(rad_diff, 0, proper)) 00180 return false; 00181 00182 CoordType sqr_dist = SquaredDistance(outer.m_center, inner.m_center); 00183 00184 return _LessEq(sqr_dist, rad_diff * rad_diff, proper); 00185 } 00186 00187 // Segment<> 00188 00189 template<const int dim> 00190 inline bool Intersect(const Segment<dim>& s, const Point<dim>& p, bool proper) 00191 { 00192 // This is only true if p lies on the line between m_p1 and m_p2 00193 00194 Vector<dim> v1 = s.m_p1 - p, v2 = s.m_p2 - p; 00195 00196 CoordType proj = Dot(v1, v2); 00197 00198 if(_Greater(proj, 0, proper)) // p is on the same side of both ends, not between them 00199 return false; 00200 00201 // Check for colinearity 00202 return Equal(proj * proj, v1.sqrMag() * v2.sqrMag()); 00203 } 00204 00205 template<const int dim> 00206 inline bool Contains(const Point<dim>& p, const Segment<dim>& s, bool proper) 00207 { 00208 return !proper && p == s.m_p1 && p == s.m_p2; 00209 } 00210 00211 template<const int dim> 00212 bool Intersect(const Segment<dim>& s, const AxisBox<dim>& b, bool proper) 00213 { 00214 // Use parametric coordinates on the line, where 0 is the location 00215 // of m_p1 and 1 is the location of m_p2 00216 00217 // Find the parametric coordinates of the portion of the line 00218 // which lies betweens b.lowerBound(i) and b.upperBound(i) for 00219 // each i. Find the intersection of those segments and the 00220 // segment (0, 1), and check if it's nonzero. 00221 00222 CoordType min = 0, max = 1; 00223 00224 for(int i = 0; i < dim; ++i) { 00225 CoordType dist = s.m_p2[i] - s.m_p1[i]; 00226 if(dist == 0) { 00227 if(_Less(s.m_p1[i], b.m_low[i], proper) 00228 || _Greater(s.m_p1[i], b.m_high[i], proper)) 00229 return false; 00230 } 00231 else { 00232 CoordType low = (b.m_low[i] - s.m_p1[i]) / dist; 00233 CoordType high = (b.m_high[i] - s.m_p1[i]) / dist; 00234 if(low > high) { 00235 CoordType tmp = high; 00236 high = low; 00237 low = tmp; 00238 } 00239 if(low > min) 00240 min = low; 00241 if(high < max) 00242 max = high; 00243 } 00244 } 00245 00246 return _LessEq(min, max, proper); 00247 } 00248 00249 template<const int dim> 00250 inline bool Contains(const Segment<dim>& s, const AxisBox<dim>& b, bool proper) 00251 { 00252 // This is only possible for zero width or zero height box, 00253 // in which case we check for containment of the endpoints. 00254 00255 bool got_difference = false; 00256 00257 for(int i = 0; i < dim; ++i) { 00258 if(b.m_low[i] == b.m_high[i]) 00259 continue; 00260 if(got_difference) 00261 return false; 00262 else // It's okay to be different on one axis 00263 got_difference = true; 00264 } 00265 00266 return Contains(s, b.m_low, proper) && 00267 (got_difference ? Contains(s, b.m_high, proper) : true); 00268 } 00269 00270 template<const int dim> 00271 inline bool Contains(const AxisBox<dim>& b, const Segment<dim>& s, bool proper) 00272 { 00273 return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper); 00274 } 00275 00276 template<const int dim> 00277 bool Intersect(const Segment<dim>& s, const Ball<dim>& b, bool proper) 00278 { 00279 Vector<dim> line = s.m_p2 - s.m_p1, offset = b.m_center - s.m_p1; 00280 00281 // First, see if the closest point on the line to the center of 00282 // the ball lies inside the segment 00283 00284 CoordType proj = Dot(line, offset); 00285 00286 // If the nearest point on the line is outside the segment, 00287 // intersection reduces to checking the nearest endpoint. 00288 00289 if(proj <= 0) 00290 return Intersect(b, s.m_p1, proper); 00291 00292 CoordType lineSqrMag = line.sqrMag(); 00293 00294 if (proj >= lineSqrMag) 00295 return Intersect(b, s.m_p2, proper); 00296 00297 Vector<dim> perp_part = offset - line * (proj / lineSqrMag); 00298 00299 return _LessEq(perp_part.sqrMag(), b.m_radius * b.m_radius, proper); 00300 } 00301 00302 template<const int dim> 00303 inline bool Contains(const Ball<dim>& b, const Segment<dim>& s, bool proper) 00304 { 00305 return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper); 00306 } 00307 00308 template<const int dim> 00309 inline bool Contains(const Segment<dim>& s, const Ball<dim>& b, bool proper) 00310 { 00311 return b.m_radius == 0 && Contains(s, b.m_center, proper); 00312 } 00313 00314 template<const int dim> 00315 bool Intersect(const Segment<dim>& s1, const Segment<dim>& s2, bool proper) 00316 { 00317 // Check that the lines that contain the segments intersect, and then check 00318 // that the intersection point lies within the segments 00319 00320 Vector<dim> v1 = s1.m_p2 - s1.m_p1, v2 = s2.m_p2 - s2.m_p1, 00321 deltav = s2.m_p1 - s1.m_p1; 00322 00323 CoordType v1sqr = v1.sqrMag(), v2sqr = v2.sqrMag(); 00324 CoordType proj12 = Dot(v1, v2), proj1delta = Dot(v1, deltav), 00325 proj2delta = Dot(v2, deltav); 00326 00327 CoordType denom = v1sqr * v2sqr - proj12 * proj12; 00328 00329 if(dim > 2 && !Equal(v2sqr * proj1delta * proj1delta + 00330 v1sqr * proj2delta * proj2delta, 00331 2 * proj12 * proj1delta * proj2delta + 00332 deltav.sqrMag() * denom)) 00333 return false; // Skew lines; don't intersect 00334 00335 if(denom > 0) { 00336 // Find the location of the intersection point in parametric coordinates, 00337 // where one end of the segment is at zero and the other at one 00338 00339 CoordType coord1 = (v2sqr * proj1delta - proj12 * proj2delta) / denom; 00340 CoordType coord2 = -(v1sqr * proj2delta - proj12 * proj1delta) / denom; 00341 00342 return _LessEq(coord1, 0, proper) && _LessEq(coord1, 1, proper) 00343 && _GreaterEq(coord2, 0, proper) && _GreaterEq(coord2, 1, proper); 00344 } 00345 else { 00346 // Parallel segments, see if one contains an endpoint of the other 00347 return Contains(s1, s2.m_p1, proper) || Contains(s1, s2.m_p2, proper) 00348 || Contains(s2, s1.m_p1, proper) || Contains(s2, s1.m_p2, proper) 00349 // Degenerate case (identical segments), nonzero length 00350 || proper && (s1.m_p1 != s1.m_p2) 00351 && ((s1.m_p1 == s2.m_p1 && s1.m_p2 == s2.m_p2) 00352 || (s1.m_p1 == s2.m_p2 && s1.m_p2 == s2.m_p1)); 00353 } 00354 } 00355 00356 template<const int dim> 00357 inline bool Contains(const Segment<dim>& s1, const Segment<dim>& s2, bool proper) 00358 { 00359 return Contains(s1, s2.m_p1, proper) && Contains(s1, s2.m_p2, proper); 00360 } 00361 00362 // RotBox<> 00363 00364 template<const int dim> 00365 inline bool Intersect(const RotBox<dim>& r, const Point<dim>& p, bool proper) 00366 { 00367 // Rotate the point into the internal coordinate system of the box 00368 00369 Vector<dim> shift = ProdInv(p - r.m_corner0, r.m_orient); 00370 00371 for(int i = 0; i < dim; ++i) { 00372 if(r.m_size[i] < 0) { 00373 if(_Less(shift[i], r.m_size[i], proper) || _Greater(shift[i], 0, proper)) 00374 return false; 00375 } 00376 else { 00377 if(_Greater(shift[i], r.m_size[i], proper) || _Less(shift[i], 0, proper)) 00378 return false; 00379 } 00380 } 00381 00382 return true; 00383 } 00384 00385 template<const int dim> 00386 inline bool Contains(const Point<dim>& p, const RotBox<dim>& r, bool proper) 00387 { 00388 if(proper) 00389 return false; 00390 00391 for(int i = 0; i < dim; ++i) 00392 if(r.m_size[i] != 0) 00393 return false; 00394 00395 return p == r.m_corner0; 00396 } 00397 00398 template<const int dim> 00399 bool Intersect(const RotBox<dim>& r, const AxisBox<dim>& b, bool proper); 00400 00401 // FIXME only have two special cases implemented 00402 00403 template<> 00404 bool Intersect<2>(const RotBox<2>& r, const AxisBox<2>& b, bool proper); 00405 template<> 00406 bool Intersect<3>(const RotBox<3>& r, const AxisBox<3>& b, bool proper); 00407 00408 template<const int dim> 00409 inline bool Contains(const RotBox<dim>& r, const AxisBox<dim>& b, bool proper) 00410 { 00411 RotMatrix<dim> m = r.m_orient.inverse(); 00412 00413 return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), 00414 RotBox<dim>(Point<dim>(b.m_low).rotate(m, r.m_corner0), 00415 b.m_high - b.m_low, m), proper); 00416 } 00417 00418 template<const int dim> 00419 inline bool Contains(const AxisBox<dim>& b, const RotBox<dim>& r, bool proper) 00420 { 00421 return Contains(b, r.boundingBox(), proper); 00422 } 00423 00424 template<const int dim> 00425 inline bool Intersect(const RotBox<dim>& r, const Ball<dim>& b, bool proper) 00426 { 00427 return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), 00428 Ball<dim>(r.m_corner0 + ProdInv(b.m_center - r.m_corner0, 00429 r.m_orient), b.m_radius), proper); 00430 } 00431 00432 template<const int dim> 00433 inline bool Contains(const RotBox<dim>& r, const Ball<dim>& b, bool proper) 00434 { 00435 return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), 00436 Ball<dim>(r.m_corner0 + ProdInv(b.m_center - r.m_corner0, 00437 r.m_orient), b.m_radius), proper); 00438 } 00439 00440 template<const int dim> 00441 inline bool Contains(const Ball<dim>& b, const RotBox<dim>& r, bool proper) 00442 { 00443 return Contains(Ball<dim>(r.m_corner0 + ProdInv(b.m_center - r.m_corner0, 00444 r.m_orient), b.m_radius), 00445 AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper); 00446 } 00447 00448 template<const int dim> 00449 inline bool Intersect(const RotBox<dim>& r, const Segment<dim>& s, bool proper) 00450 { 00451 Point<dim> p1 = r.m_corner0 + ProdInv(s.m_p1 - r.m_corner0, r.m_orient); 00452 Point<dim> p2 = r.m_corner0 + ProdInv(s.m_p2 - r.m_corner0, r.m_orient); 00453 00454 return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), 00455 Segment<dim>(p1, p2), proper); 00456 } 00457 00458 template<const int dim> 00459 inline bool Contains(const RotBox<dim>& r, const Segment<dim>& s, bool proper) 00460 { 00461 Point<dim> p1 = r.m_corner0 + ProdInv(s.m_p1 - r.m_corner0, r.m_orient); 00462 Point<dim> p2 = r.m_corner0 + ProdInv(s.m_p2 - r.m_corner0, r.m_orient); 00463 00464 return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), 00465 Segment<dim>(p1, p2), proper); 00466 } 00467 00468 template<const int dim> 00469 inline bool Contains(const Segment<dim>& s, const RotBox<dim>& r, bool proper) 00470 { 00471 Point<dim> p1 = r.m_corner0 + ProdInv(s.m_p1 - r.m_corner0, r.m_orient); 00472 Point<dim> p2 = r.m_corner0 + ProdInv(s.m_p2 - r.m_corner0, r.m_orient); 00473 00474 return Contains(Segment<dim>(p1, p2), 00475 AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper); 00476 } 00477 00478 template<const int dim> 00479 inline bool Intersect(const RotBox<dim>& r1, const RotBox<dim>& r2, bool proper) 00480 { 00481 return Intersect(RotBox<dim>(r1).rotatePoint(r2.m_orient.inverse(), 00482 r2.m_corner0), 00483 AxisBox<dim>(r2.m_corner0, r2.m_corner0 + r2.m_size), proper); 00484 } 00485 00486 template<const int dim> 00487 inline bool Contains(const RotBox<dim>& outer, const RotBox<dim>& inner, bool proper) 00488 { 00489 return Contains(AxisBox<dim>(outer.m_corner0, outer.m_corner0 + outer.m_size), 00490 RotBox<dim>(inner).rotatePoint(outer.m_orient.inverse(), 00491 outer.m_corner0), proper); 00492 } 00493 00494 // Polygon<> intersection functions are in polygon_funcs.h, to avoid 00495 // unnecessary inclusion of <vector> 00496 00497 } // namespace WFMath 00498 00499 #endif // WFMATH_INTERSECT_H

Generated on Thu Jul 29 07:09:56 2004 for WFMath by doxygen 1.3.7