Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

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

Generated on Wed May 28 09:20:31 2003 for WFMath by doxygen1.3-rc3