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