• Main Page
  • Namespaces
  • Classes
  • Files

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 #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 + numeric_constants<CoordType>::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 + numeric_constants<CoordType>::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 template<int dim>
00406 inline bool Contains(const RotBox<dim>& r, const AxisBox<dim>& b, bool proper)
00407 {
00408   RotMatrix<dim> m = r.m_orient.inverse();
00409 
00410   return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00411                   RotBox<dim>(Point<dim>(b.m_low).rotate(m, r.m_corner0),
00412                               b.m_high - b.m_low, m), proper);
00413 }
00414 
00415 template<int dim>
00416 inline bool Contains(const AxisBox<dim>& b, const RotBox<dim>& r, bool proper)
00417 {
00418   return Contains(b, r.boundingBox(), proper);
00419 }
00420 
00421 template<int dim>
00422 inline bool Intersect(const RotBox<dim>& r, const Ball<dim>& b, bool proper)
00423 {
00424   return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00425                   Ball<dim>(r.m_corner0 + ProdInv(b.m_center - r.m_corner0,
00426                             r.m_orient), b.m_radius), proper);
00427 }
00428 
00429 template<int dim>
00430 inline bool Contains(const RotBox<dim>& r, const Ball<dim>& b, bool proper)
00431 {
00432   return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00433                   Ball<dim>(r.m_corner0 + ProdInv(b.m_center - r.m_corner0,
00434                             r.m_orient), b.m_radius), proper);
00435 }
00436 
00437 template<int dim>
00438 inline bool Contains(const Ball<dim>& b, const RotBox<dim>& r, bool proper)
00439 {
00440   return Contains(Ball<dim>(r.m_corner0 + ProdInv(b.m_center - r.m_corner0,
00441                             r.m_orient), b.m_radius),
00442                   AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
00443 }
00444 
00445 template<int dim>
00446 inline bool Intersect(const RotBox<dim>& r, const Segment<dim>& s, bool proper)
00447 {
00448   Point<dim> p1 = r.m_corner0 + ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
00449   Point<dim> p2 = r.m_corner0 + ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
00450 
00451   return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00452                    Segment<dim>(p1, p2), proper);
00453 }
00454 
00455 template<int dim>
00456 inline bool Contains(const RotBox<dim>& r, const Segment<dim>& s, bool proper)
00457 {
00458   Point<dim> p1 = r.m_corner0 + ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
00459   Point<dim> p2 = r.m_corner0 + ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
00460 
00461   return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00462                   Segment<dim>(p1, p2), proper);
00463 }
00464 
00465 template<int dim>
00466 inline bool Contains(const Segment<dim>& s, const RotBox<dim>& r, bool proper)
00467 {
00468   Point<dim> p1 = r.m_corner0 + ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
00469   Point<dim> p2 = r.m_corner0 + ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
00470 
00471   return Contains(Segment<dim>(p1, p2),
00472                   AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
00473 }
00474 
00475 template<int dim>
00476 inline bool Intersect(const RotBox<dim>& r1, const RotBox<dim>& r2, bool proper)
00477 {
00478   return Intersect(RotBox<dim>(r1).rotatePoint(r2.m_orient.inverse(),
00479                                                r2.m_corner0),
00480                    AxisBox<dim>(r2.m_corner0, r2.m_corner0 + r2.m_size), proper);
00481 }
00482 
00483 template<int dim>
00484 inline bool Contains(const RotBox<dim>& outer, const RotBox<dim>& inner, bool proper)
00485 {
00486   return Contains(AxisBox<dim>(outer.m_corner0, outer.m_corner0 + outer.m_size),
00487                   RotBox<dim>(inner).rotatePoint(outer.m_orient.inverse(),
00488                                                  outer.m_corner0), proper);
00489 }
00490 
00491 // Polygon<> intersection functions are in polygon_funcs.h, to avoid
00492 // unnecessary inclusion of <vector>
00493 
00494 } // namespace WFMath
00495 
00496 #endif  // WFMATH_INTERSECT_H