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