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