WFMath  1.0.2
polygon.h
00001 // polygon.h (A 2D polygon embeded in a <dim> dimensional space)
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 // Author: Ron Steinke
00025 
00026 #ifndef WFMATH_POLYGON_H
00027 #define WFMATH_POLYGON_H
00028 
00029 #include <wfmath/const.h>
00030 #include <wfmath/axisbox.h>
00031 #include <wfmath/ball.h>
00032 #include <wfmath/quaternion.h>
00033 
00034 #include <vector>
00035 
00036 namespace WFMath {
00037 
00038 template<int dim> class Polygon;
00039 
00040 template<int dim>
00041 std::ostream& operator<<(std::ostream& os, const Polygon<dim>& r);
00042 template<int dim>
00043 std::istream& operator>>(std::istream& is, Polygon<dim>& r);
00044 
00046 template<>
00047 class Polygon<2>
00048 {
00049  public:
00050   Polygon() : m_points() {}
00051   Polygon(const Polygon& p) : m_points(p.m_points) {}
00053   explicit Polygon(const AtlasInType& a) : m_points() {fromAtlas(a);}
00054 
00055   ~Polygon() {}
00056 
00057   friend std::ostream& operator<< <2>(std::ostream& os, const Polygon& p);
00058   friend std::istream& operator>> <2>(std::istream& is, Polygon& p);
00059 
00060   
00062   AtlasOutType toAtlas() const;
00064   void fromAtlas(const AtlasInType& a);
00065   
00066   Polygon& operator=(const Polygon& p)
00067   {m_points = p.m_points; return *this;}
00068 
00069   bool isEqualTo(const Polygon& p, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;
00070 
00071   bool operator==(const Polygon& p) const       {return isEqualTo(p);}
00072   bool operator!=(const Polygon& p) const       {return !isEqualTo(p);}
00073 
00074   bool isValid() const;
00075 
00076   // Descriptive characteristics
00077 
00078   size_t numCorners() const {return m_points.size();}
00079   Point<2> getCorner(size_t i) const {return m_points[i];}
00080   Point<2> getCenter() const {return Barycenter(m_points);}
00081 
00082   // For a Polygon<2>, addCorner() and moveCorner() always succeed.
00083   // The return values are present for the sake of a unified template
00084   // interface, and the epsilon argument is ignored
00085 
00086   // Add before i'th corner, zero is beginning, numCorners() is end
00087   bool addCorner(size_t i, const Point<2>& p, CoordType = numeric_constants<CoordType>::epsilon())
00088   {m_points.insert(m_points.begin() + i, p); return true;}
00089 
00090   // Remove the i'th corner
00091   void removeCorner(size_t i) {m_points.erase(m_points.begin() + i);}
00092 
00093   // Move the i'th corner to p
00094   bool moveCorner(size_t i, const Point<2>& p, CoordType = numeric_constants<CoordType>::epsilon())
00095   {m_points[i] = p; return true;}
00096 
00097   // Remove all points
00098   void clear()  {m_points.clear();}
00099 
00100   const Point<2>& operator[](size_t i) const {return m_points[i];}
00101   Point<2>& operator[](size_t i)                  {return m_points[i];}
00102 
00103   void resize(std::vector<Point<2> >::size_type size) {m_points.resize(size);}
00104 
00105   // Movement functions
00106 
00107   Polygon& shift(const Vector<2>& v);
00108   Polygon& moveCornerTo(const Point<2>& p, size_t corner)
00109   {return shift(p - getCorner(corner));}
00110   Polygon& moveCenterTo(const Point<2>& p)
00111   {return shift(p - getCenter());}
00112 
00113   Polygon& rotateCorner(const RotMatrix<2>& m, size_t corner)
00114   {rotatePoint(m, getCorner(corner)); return *this;}
00115   Polygon& rotateCenter(const RotMatrix<2>& m)
00116   {rotatePoint(m, getCenter()); return *this;}
00117   Polygon& rotatePoint(const RotMatrix<2>& m, const Point<2>& p);
00118 
00119   // Intersection functions
00120 
00121   AxisBox<2> boundingBox() const {return BoundingBox(m_points);}
00122   Ball<2> boundingSphere() const {return BoundingSphere(m_points);}
00123   Ball<2> boundingSphereSloppy() const {return BoundingSphereSloppy(m_points);}
00124 
00125   Polygon toParentCoords(const Point<2>& origin,
00126       const RotMatrix<2>& rotation = RotMatrix<2>().identity()) const;
00127   Polygon toParentCoords(const AxisBox<2>& coords) const;
00128   Polygon toParentCoords(const RotBox<2>& coords) const;
00129 
00130   // toLocal is just like toParent, expect we reverse the order of
00131   // translation and rotation and use the opposite sense of the rotation
00132   // matrix
00133 
00134   Polygon toLocalCoords(const Point<2>& origin,
00135       const RotMatrix<2>& rotation = RotMatrix<2>().identity()) const;
00136   Polygon toLocalCoords(const AxisBox<2>& coords) const;
00137   Polygon toLocalCoords(const RotBox<2>& coords) const;
00138 
00139   friend bool Intersect<2>(const Polygon& r, const Point<2>& p, bool proper);
00140   friend bool Contains<2>(const Point<2>& p, const Polygon& r, bool proper);
00141 
00142   friend bool Intersect<2>(const Polygon& p, const AxisBox<2>& b, bool proper);
00143   friend bool Contains<2>(const Polygon& p, const AxisBox<2>& b, bool proper);
00144   friend bool Contains<2>(const AxisBox<2>& b, const Polygon& p, bool proper);
00145 
00146   friend bool Intersect<2>(const Polygon& p, const Ball<2>& b, bool proper);
00147   friend bool Contains<2>(const Polygon& p, const Ball<2>& b, bool proper);
00148   friend bool Contains<2>(const Ball<2>& b, const Polygon& p, bool proper);
00149 
00150   friend bool Intersect<2>(const Polygon& r, const Segment<2>& s, bool proper);
00151   friend bool Contains<2>(const Polygon& p, const Segment<2>& s, bool proper);
00152   friend bool Contains<2>(const Segment<2>& s, const Polygon& p, bool proper);
00153 
00154   friend bool Intersect<2>(const Polygon& p, const RotBox<2>& r, bool proper);
00155   friend bool Contains<2>(const Polygon& p, const RotBox<2>& r, bool proper);
00156   friend bool Contains<2>(const RotBox<2>& r, const Polygon& p, bool proper);
00157 
00158   friend bool Intersect<2>(const Polygon& p1, const Polygon& p2, bool proper);
00159   friend bool Contains<2>(const Polygon& outer, const Polygon& inner, bool proper);
00160 
00161 private:
00162   std::vector<Point<2> > m_points;
00163   typedef std::vector<Point<2> >::iterator theIter;
00164   typedef std::vector<Point<2> >::const_iterator theConstIter;
00165 
00166 };
00167 
00168 // Helper classes, to keep track of the orientation of
00169 // a 2D polygon in dim dimensions
00170 
00171 typedef enum {
00172   _WFMATH_POLY2REORIENT_NONE,
00173   _WFMATH_POLY2REORIENT_CLEAR_AXIS2,
00174   _WFMATH_POLY2REORIENT_CLEAR_BOTH_AXES,
00175   _WFMATH_POLY2REORIENT_MOVE_AXIS2_TO_AXIS1,
00176   _WFMATH_POLY2REORIENT_SCALE1_CLEAR2
00177 } _Poly2ReorientType;
00178 
00179 // Reorient a 2D polygon to match a change in the basis
00180 // used by _Poly2Orient
00181 class _Poly2Reorient
00182 {
00183 public:
00184   _Poly2Reorient(_Poly2ReorientType type, CoordType scale = 0.0)
00185   : m_type(type), m_scale(scale) {}
00186   ~_Poly2Reorient() {}
00187 
00188   void reorient(Polygon<2>& poly, size_t skip = std::numeric_limits<size_t>::max()) const;
00189 
00190 private:
00191   _Poly2ReorientType m_type;
00192   CoordType m_scale;
00193 };
00194 
00195 template<int dim> class _Poly2Orient;
00196 
00197 struct _Poly2OrientIntersectData {
00198   int dim;
00199   Point<2> p1, p2;
00200   Vector<2> v1, v2, off;
00201   bool o1_is_line, o2_is_line;
00202 };
00203 
00204 // Finds the intersection of the two planes, returns the
00205 // dimension of the intersection space, the rest of the arguments
00206 // are various information returned depending on the dimension of
00207 // the intersection
00208 template<int dim>
00209 int  _Intersect(const _Poly2Orient<dim> &, const _Poly2Orient<dim> &,
00210     _Poly2OrientIntersectData &);
00211 
00212 // Keep track of the orientation of a 2D polygon in dim dimensions
00213 template<int dim>
00214 class _Poly2Orient
00215 {
00216 public:
00217   _Poly2Orient() : m_origin() {}
00218   _Poly2Orient(const _Poly2Orient& p) : m_origin() {operator=(p);}
00219   ~_Poly2Orient() {}
00220 
00221   _Poly2Orient& operator=(const _Poly2Orient& p);
00222 
00223   // Convert a point in the 2D polygon to a point in dim dimensional space
00224   Point<dim> convert(const Point<2>& p) const;
00225 
00226   // Try to convert a point from dim dimensions into 2D, expanding the
00227   // basis if necessary. Returns true on success. On failure, the
00228   // basis is unchanged.
00229   bool expand(const Point<dim>& pd, Point<2>& p2, CoordType epsilon = numeric_constants<CoordType>::epsilon());
00230 
00231   // Reduce the basis to the minimum necessary to span the points in
00232   // poly (with the exception of skip). Returns _Poly2Reorient, which needs
00233   // to be used to reorient the points to match the new basis.
00234   _Poly2Reorient reduce(const Polygon<2>& poly, size_t skip = std::numeric_limits<size_t>::max());
00235 
00236   void shift(const Vector<dim>& v) {if(m_origin.isValid()) m_origin += v;}
00237   void rotate(const RotMatrix<dim>& m, const Point<dim>& p);
00238   // Rotates about the point which corresponds to "p" in the oriented plane
00239   void rotate2(const RotMatrix<dim>& m, const Point<2>& p);
00240 
00241 //3D only
00242   void rotate(const Quaternion& q, const Point<3>& p);
00243   // Rotates about the point which corresponds to "p" in the oriented plane
00244   void rotate2(const Quaternion& q, const Point<2>& p);
00245 
00246   _Poly2Orient toParentCoords(const Point<dim>& origin,
00247       const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00248   {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(origin, rotation);
00249     p.m_axes[0].rotate(rotation); p.m_axes[1].rotate(rotation); return p;}
00250   _Poly2Orient toParentCoords(const AxisBox<dim>& coords) const
00251   {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords); return p;}
00252   _Poly2Orient toParentCoords(const RotBox<dim>& coords) const
00253   {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords);
00254     p.m_axes[0].rotate(coords.orientation());
00255     p.m_axes[1].rotate(coords.orientation()); return p;}
00256 
00257   // toLocal is just like toParent, expect we reverse the order of
00258   // translation and rotation and use the opposite sense of the rotation
00259   // matrix
00260 
00261   _Poly2Orient toLocalCoords(const Point<dim>& origin,
00262       const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00263   {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(origin, rotation);
00264     p.m_axes[0] = rotation * p.m_axes[0];
00265     p.m_axes[1] = rotation * p.m_axes[1]; return p;}
00266   _Poly2Orient toLocalCoords(const AxisBox<dim>& coords) const
00267   {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords); return p;}
00268   _Poly2Orient toLocalCoords(const RotBox<dim>& coords) const
00269   {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords);
00270     p.m_axes[0] = coords.orientation() * p.m_axes[0];
00271     p.m_axes[1] = coords.orientation() * p.m_axes[1]; return p;}
00272 
00273   // 3D only
00274   _Poly2Orient<3> toParentCoords(const Point<3>& origin, const Quaternion& rotation) const
00275   {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(origin, rotation);
00276     p.m_axes[0].rotate(rotation); p.m_axes[0].rotate(rotation); return p;}
00277   _Poly2Orient<3> toLocalCoords(const Point<3>& origin, const Quaternion& rotation) const
00278   {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(origin, rotation);
00279     p.m_axes[0].rotate(rotation.inverse());
00280     p.m_axes[0].rotate(rotation.inverse()); return p;}
00281 
00282   // Gives the offset from pd to the space spanned by
00283   // the basis, and puts the nearest point in p2.
00284   Vector<dim> offset(const Point<dim>& pd, Point<2>& p2) const;
00285 
00286   // Like offset, but returns true if the point is in the plane
00287   bool checkContained(const Point<dim>& pd, Point<2> & p2) const;
00288 
00289   // Check if the AxisBox intersects the spanned space, and if so
00290   // return a point in the intersection.
00291   bool checkIntersect(const AxisBox<dim>& b, Point<2>& p2, bool proper) const;
00292 
00293   friend int  _Intersect<dim>(const _Poly2Orient<dim> &, const _Poly2Orient<dim> &,
00294             _Poly2OrientIntersectData &);
00295 
00296 private:
00297   // special case of the above when both axes are valid
00298   bool checkIntersectPlane(const AxisBox<dim>& b, Point<2>& p2, bool proper) const;
00299 
00300   Point<dim> m_origin;
00301   Vector<dim> m_axes[2]; // Normalized to unit length
00302 };
00303 
00305 template<int dim = 3>
00306 class Polygon
00307 {
00308 public:
00309   Polygon() : m_orient(), m_poly() {}
00310   Polygon(const Polygon& p) : m_orient(p.m_orient), m_poly(p.m_poly) {}
00311 
00312   ~Polygon() {}
00313 
00314   friend std::ostream& operator<< <dim>(std::ostream& os, const Polygon& p);
00315   friend std::istream& operator>> <dim>(std::istream& is, Polygon& p);
00316 
00317   Polygon& operator=(const Polygon& p)
00318   {m_orient = p.m_orient; m_poly = p.m_poly; return *this;}
00319 
00320   bool isEqualTo(const Polygon& p2, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;
00321 
00322   bool operator==(const Polygon& p) const       {return isEqualTo(p);}
00323   bool operator!=(const Polygon& p) const       {return !isEqualTo(p);}
00324 
00325   bool isValid() const {return m_poly.isValid();}
00326 
00327   // Descriptive characteristics
00328 
00329   size_t numCorners() const {return m_poly.numCorners();}
00330   Point<dim> getCorner(size_t i) const {return m_orient.convert(m_poly[i]);}
00331   Point<dim> getCenter() const {return m_orient.convert(m_poly.getCenter());}
00332 
00333   // The failure of the following functions does not invalidate the
00334   // polygon, but merely leaves it unchaged.
00335 
00336   // Add before i'th corner, zero is beginning, numCorners() is end
00337   // Only succeeds if p lies in a plane with all current points
00338   bool addCorner(size_t i, const Point<dim>& p, CoordType epsilon = numeric_constants<CoordType>::epsilon());
00339 
00340   // Remove the i'th corner
00341   void removeCorner(size_t i);
00342 
00343   // Move the i'th corner to p, only succeeds if new location
00344   // lies in the same plane as all the other points. Note that,
00345   // under certain circumstances, this plane may not contain the
00346   // original location of the point.
00347   bool moveCorner(size_t i, const Point<dim>& p, CoordType epsilon = numeric_constants<CoordType>::epsilon());
00348 
00349   // Remove all points
00350   void clear()  {m_poly.clear(); m_orient = _Poly2Orient<dim>();}
00351 
00352   // Movement functions
00353 
00354   Polygon& shift(const Vector<dim>& v)
00355   {m_orient.shift(v); return *this;}
00356   Polygon& moveCornerTo(const Point<dim>& p, size_t corner)
00357   {return shift(p - getCorner(corner));}
00358   Polygon& moveCenterTo(const Point<dim>& p)
00359   {return shift(p - getCenter());}
00360 
00361   Polygon& rotateCorner(const RotMatrix<dim>& m, size_t corner)
00362   {m_orient.rotate2(m, m_poly[corner]); return *this;}
00363   Polygon& rotateCenter(const RotMatrix<dim>& m)
00364   {if(m_poly.numCorners() > 0)
00365     m_orient.rotate2(m, m_poly.getCenter());
00366   return *this;}
00367   Polygon& rotatePoint(const RotMatrix<dim>& m, const Point<dim>& p)
00368   {m_orient.rotate(m, p); return *this;}
00369 
00370   // 3D rotation functions
00371   Polygon<3>& rotateCorner(const Quaternion& q, size_t corner)
00372   {m_orient.rotate2(q, m_poly[corner]); return *this;}
00373   Polygon<3>& rotateCenter(const Quaternion& q)
00374   {if(m_poly.numCorners() > 0)
00375     m_orient.rotate2(q, m_poly.getCenter());
00376   return *this;}
00377   Polygon<3>& rotatePoint(const Quaternion& q, const Point<3>& p)
00378   {m_orient.rotate(q, p); return *this;}
00379 
00380   // Intersection functions
00381 
00382   AxisBox<dim> boundingBox() const;
00383   Ball<dim> boundingSphere() const;
00384   Ball<dim> boundingSphereSloppy() const;
00385 
00386   Polygon toParentCoords(const Point<dim>& origin,
00387       const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00388         {Polygon p(*this); p.m_orient = m_orient.toParentCoords(origin, rotation); return p;}
00389   Polygon toParentCoords(const AxisBox<dim>& coords) const
00390         {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
00391   Polygon toParentCoords(const RotBox<dim>& coords) const
00392         {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
00393 
00394   // toLocal is just like toParent, expect we reverse the order of
00395   // translation and rotation and use the opposite sense of the rotation
00396   // matrix
00397 
00398   Polygon toLocalCoords(const Point<dim>& origin,
00399       const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00400         {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(origin, rotation); return p;}
00401   Polygon toLocalCoords(const AxisBox<dim>& coords) const
00402         {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
00403   Polygon toLocalCoords(const RotBox<dim>& coords) const
00404         {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
00405 
00406   // 3D only
00407   Polygon<3> toParentCoords(const Point<3>& origin, const Quaternion& rotation) const
00408         {Polygon<3> p(*this); p.m_orient = m_orient.toParentCoords(origin, rotation); return p;}
00409   Polygon<3> toLocalCoords(const Point<3>& origin, const Quaternion& rotation) const
00410         {Polygon<3> p(*this); p.m_orient = m_orient.toLocalCoords(origin, rotation); return p;}
00411 
00412   friend bool Intersect<dim>(const Polygon& r, const Point<dim>& p, bool proper);
00413   friend bool Contains<dim>(const Point<dim>& p, const Polygon& r, bool proper);
00414 
00415   friend bool Intersect<dim>(const Polygon& p, const AxisBox<dim>& b, bool proper);
00416   friend bool Contains<dim>(const Polygon& p, const AxisBox<dim>& b, bool proper);
00417   friend bool Contains<dim>(const AxisBox<dim>& b, const Polygon& p, bool proper);
00418 
00419   friend bool Intersect<dim>(const Polygon& p, const Ball<dim>& b, bool proper);
00420   friend bool Contains<dim>(const Polygon& p, const Ball<dim>& b, bool proper);
00421   friend bool Contains<dim>(const Ball<dim>& b, const Polygon& p, bool proper);
00422 
00423   friend bool Intersect<dim>(const Polygon& r, const Segment<dim>& s, bool proper);
00424   friend bool Contains<dim>(const Polygon& p, const Segment<dim>& s, bool proper);
00425   friend bool Contains<dim>(const Segment<dim>& s, const Polygon& p, bool proper);
00426 
00427   friend bool Intersect<dim>(const Polygon& p, const RotBox<dim>& r, bool proper);
00428   friend bool Contains<dim>(const Polygon& p, const RotBox<dim>& r, bool proper);
00429   friend bool Contains<dim>(const RotBox<dim>& r, const Polygon& p, bool proper);
00430 
00431   friend bool Intersect<dim>(const Polygon& p1, const Polygon& p2, bool proper);
00432   friend bool Contains<dim>(const Polygon& outer, const Polygon& inner, bool proper);
00433 
00434  private:
00435 
00436   _Poly2Orient<dim> m_orient;
00437   Polygon<2> m_poly;
00438 };
00439 
00440 template<int dim>
00441 inline bool Polygon<dim>::addCorner(size_t i, const Point<dim>& p, CoordType epsilon)
00442 {
00443   Point<2> p2;
00444   bool succ = m_orient.expand(p, p2, epsilon);
00445   if(succ)
00446     m_poly.addCorner(i, p2, epsilon);
00447   return succ;
00448 }
00449 
00450 template<int dim>
00451 inline void Polygon<dim>::removeCorner(size_t i)
00452 {
00453   m_poly.removeCorner(i);
00454   _Poly2Reorient r = m_orient.reduce(m_poly);
00455   r.reorient(m_poly);
00456 }
00457 
00458 template<int dim>
00459 inline bool Polygon<dim>::moveCorner(size_t i, const Point<dim>& p, CoordType epsilon)
00460 {
00461   _Poly2Orient<dim> try_orient = m_orient;
00462   _Poly2Reorient r = try_orient.reduce(m_poly, i);
00463   Point<2> p2;
00464 
00465   if(!try_orient.expand(p, p2, epsilon))
00466     return false;
00467 
00468   r.reorient(m_poly, i);
00469   m_poly[i] = p2;
00470   m_orient = try_orient;
00471 
00472   return true;
00473 }
00474 
00475 } // namespace WFMath
00476 
00477 #endif  // WFMATH_POLYGON_H