WFMath 0.3.11

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 // 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