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 Sun Dec 16 11:36:53 2007 for WFMath by  doxygen 1.5.2