00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef WFMATH_POLYGON_H
00027 #define WFMATH_POLYGON_H
00028
00029 #include <wfmath/const.h>
00030 #include <wfmath/vector.h>
00031 #include <wfmath/point.h>
00032 #include <wfmath/rotmatrix.h>
00033 #include <wfmath/axisbox.h>
00034 #include <wfmath/rotbox.h>
00035 #include <wfmath/ball.h>
00036 #include <wfmath/intersect_decls.h>
00037
00038 #include <vector>
00039
00040 namespace WFMath {
00041
00042 template<const int dim> class Polygon;
00043
00044 template<const int dim>
00045 std::ostream& operator<<(std::ostream& os, const Polygon<dim>& r);
00046 template<const int dim>
00047 std::istream& operator>>(std::istream& is, Polygon<dim>& r);
00048
00050 template<>
00051 class Polygon<2>
00052 {
00053 public:
00054 Polygon() {}
00055 Polygon(const Polygon& p) : m_points(p.m_points) {}
00056
00057 ~Polygon() {}
00058 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00059 friend std::ostream& operator<< <2>(std::ostream& os, const Polygon& p);
00060 friend std::istream& operator>> <2>(std::istream& is, Polygon& p);
00061 #endif
00062 Polygon& operator=(const Polygon& p)
00063 {m_points = p.m_points; return *this;}
00064
00065 bool isEqualTo(const Polygon& p, double epsilon = WFMATH_EPSILON) const;
00066
00067 bool operator==(const Polygon& p) const {return isEqualTo(p);}
00068 bool operator!=(const Polygon& p) const {return !isEqualTo(p);}
00069
00070 bool isValid() const;
00071
00072
00073
00074 int numCorners() const {return m_points.size();}
00075 Point<2> getCorner(int i) const
00076 {assert(i >= 0 && ((unsigned int) i) < m_points.size()); return m_points[i];}
00077 #ifndef WFMATH_NO_TEMPLATES_AS_TEMPLATE_PARAMETERS
00078 Point<2> getCenter() const {return Barycenter(m_points);}
00079 #endif
00080
00081
00082
00083
00084
00085
00086 bool addCorner(int i, const Point<2>& p, double epsilon = WFMATH_EPSILON)
00087 {m_points.insert(m_points.begin() + i, p); return true;}
00088
00089
00090 void removeCorner(int i) {m_points.erase(m_points.begin() + i);}
00091
00092
00093 bool moveCorner(int i, const Point<2>& p, double epsilon = WFMATH_EPSILON)
00094 {m_points[i] = p; return true;}
00095
00096
00097 void clear() {m_points.clear();}
00098
00099 const Point<2>& operator[](int i) const {return m_points[i];}
00100 Point<2>& operator[](int i) {return m_points[i];}
00101
00102 void resize(unsigned int size) {m_points.resize(size);}
00103
00104
00105
00106 Polygon& shift(const Vector<2>& v);
00107 Polygon& moveCornerTo(const Point<2>& p, int corner)
00108 {return shift(p - getCorner(corner));}
00109 #ifndef WFMATH_NO_TEMPLATES_AS_TEMPLATE_PARAMETERS
00110 Polygon& moveCenterTo(const Point<2>& p)
00111 {return shift(p - getCenter());}
00112 #endif
00113
00114 Polygon& rotateCorner(const RotMatrix<2>& m, int corner)
00115 {rotatePoint(m, getCorner(corner)); return *this;}
00116 #ifndef WFMATH_NO_TEMPLATES_AS_TEMPLATE_PARAMETERS
00117 Polygon& rotateCenter(const RotMatrix<2>& m)
00118 {rotatePoint(m, getCenter()); return *this;}
00119 #endif
00120 Polygon& rotatePoint(const RotMatrix<2>& m, const Point<2>& p);
00121
00122
00123
00124 #ifndef WFMATH_NO_TEMPLATES_AS_TEMPLATE_PARAMETERS
00125 AxisBox<2> boundingBox() const {return BoundingBox(m_points);}
00126 Ball<2> boundingSphere() const {return BoundingSphere(m_points);}
00127 Ball<2> boundingSphereSloppy() const {return BoundingSphereSloppy(m_points);}
00128 #endif
00129
00130 Polygon toParentCoords(const Point<2>& origin,
00131 const RotMatrix<2>& rotation = RotMatrix<2>().identity()) const;
00132 Polygon toParentCoords(const AxisBox<2>& coords) const;
00133 Polygon toParentCoords(const RotBox<2>& coords) const;
00134
00135
00136
00137
00138
00139 Polygon toLocalCoords(const Point<2>& origin,
00140 const RotMatrix<2>& rotation = RotMatrix<2>().identity()) const;
00141 Polygon toLocalCoords(const AxisBox<2>& coords) const;
00142 Polygon toLocalCoords(const RotBox<2>& coords) const;
00143
00144 friend bool Intersect<2>(const Polygon& r, const Point<2>& p, bool proper);
00145 friend bool Contains<2>(const Point<2>& p, const Polygon& r, bool proper);
00146
00147 friend bool Intersect<2>(const Polygon& p, const AxisBox<2>& b, bool proper);
00148 friend bool Contains<2>(const Polygon& p, const AxisBox<2>& b, bool proper);
00149 friend bool Contains<2>(const AxisBox<2>& b, const Polygon& p, bool proper);
00150
00151 friend bool Intersect<2>(const Polygon& p, const Ball<2>& b, bool proper);
00152 friend bool Contains<2>(const Polygon& p, const Ball<2>& b, bool proper);
00153 friend bool Contains<2>(const Ball<2>& b, const Polygon& p, bool proper);
00154
00155 friend bool Intersect<2>(const Polygon& r, const Segment<2>& s, bool proper);
00156 friend bool Contains<2>(const Polygon& p, const Segment<2>& s, bool proper);
00157 friend bool Contains<2>(const Segment<2>& s, const Polygon& p, bool proper);
00158
00159 friend bool Intersect<2>(const Polygon& p, const RotBox<2>& r, bool proper);
00160 friend bool Contains<2>(const Polygon& p, const RotBox<2>& r, bool proper);
00161 friend bool Contains<2>(const RotBox<2>& r, const Polygon& p, bool proper);
00162
00163 friend bool Intersect<2>(const Polygon& p1, const Polygon& p2, bool proper);
00164 friend bool Contains<2>(const Polygon& outer, const Polygon& inner, bool proper);
00165
00166 private:
00167 std::vector<Point<2> > m_points;
00168 typedef std::vector<Point<2> >::iterator theIter;
00169 typedef std::vector<Point<2> >::const_iterator theConstIter;
00170
00171 };
00172
00173
00174
00175
00176 typedef enum {
00177 _WFMATH_POLY2REORIENT_NONE,
00178 _WFMATH_POLY2REORIENT_CLEAR_AXIS2,
00179 _WFMATH_POLY2REORIENT_CLEAR_BOTH_AXES,
00180 _WFMATH_POLY2REORIENT_MOVE_AXIS2_TO_AXIS1,
00181 _WFMATH_POLY2REORIENT_SCALE1_CLEAR2
00182 } _Poly2ReorientType;
00183
00184
00185
00186 class _Poly2Reorient
00187 {
00188 public:
00189 _Poly2Reorient(_Poly2ReorientType type, CoordType scale = 0.0)
00190 : m_type(type), m_scale(scale) {}
00191 ~_Poly2Reorient() {}
00192
00193 void reorient(Polygon<2>& poly, int skip = -1) const;
00194
00195 private:
00196 _Poly2ReorientType m_type;
00197 CoordType m_scale;
00198 };
00199
00200 template<const int dim> class _Poly2Orient;
00201
00202 struct _Poly2OrientIntersectData {
00203 int dim;
00204 Point<2> p1, p2;
00205 Vector<2> v1, v2, off;
00206 bool o1_is_line, o2_is_line;
00207 };
00208
00209
00210
00211
00212
00213 template<const int dim>
00214 int _Intersect(const _Poly2Orient<dim> &, const _Poly2Orient<dim> &,
00215 _Poly2OrientIntersectData &);
00216
00217
00218 template<const int dim>
00219 class _Poly2Orient
00220 {
00221 public:
00222 _Poly2Orient() {}
00223 _Poly2Orient(const _Poly2Orient& p) {operator=(p);}
00224 ~_Poly2Orient() {}
00225
00226 _Poly2Orient& operator=(const _Poly2Orient& p);
00227
00228
00229 Point<dim> convert(const Point<2>& p) const;
00230
00231
00232
00233
00234 bool expand(const Point<dim>& pd, Point<2>& p2, double epsilon = WFMATH_EPSILON);
00235
00236
00237
00238
00239 _Poly2Reorient reduce(const Polygon<2>& poly, int skip = -1);
00240
00241 void shift(const Vector<dim>& v) {if(m_origin.isValid()) m_origin += v;}
00242 void rotate(const RotMatrix<dim>& m, const Point<dim>& p);
00243
00244 void rotate2(const RotMatrix<dim>& m, const Point<2>& p);
00245
00246
00247 void rotate(const Quaternion& q, const Point<3>& p);
00248
00249 void rotate2(const Quaternion& q, const Point<2>& p);
00250
00251 _Poly2Orient toParentCoords(const Point<dim>& origin,
00252 const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00253 {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(origin, rotation);
00254 p.m_axes[0] *= rotation; p.m_axes[1] *= rotation; return p;}
00255 _Poly2Orient toParentCoords(const AxisBox<dim>& coords) const
00256 {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords); return p;}
00257 _Poly2Orient toParentCoords(const RotBox<dim>& coords) const
00258 {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords);
00259 p.m_axes[0] *= coords.orientation();
00260 p.m_axes[1] *= coords.orientation(); return p;}
00261
00262
00263
00264
00265
00266 _Poly2Orient toLocalCoords(const Point<dim>& origin,
00267 const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00268 {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(origin, rotation);
00269 p.m_axes[0] = rotation * p.m_axes[0];
00270 p.m_axes[1] = rotation * p.m_axes[1]; return p;}
00271 _Poly2Orient toLocalCoords(const AxisBox<dim>& coords) const
00272 {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords); return p;}
00273 _Poly2Orient toLocalCoords(const RotBox<dim>& coords) const
00274 {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords);
00275 p.m_axes[0] = coords.orientation() * p.m_axes[0];
00276 p.m_axes[1] = coords.orientation() * p.m_axes[1]; return p;}
00277
00278
00279 _Poly2Orient<3> toParentCoords(const Point<3>& origin, const Quaternion& rotation) const
00280 {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(origin, rotation);
00281 p.m_axes[0].rotate(rotation); p.m_axes[0].rotate(rotation); return p;}
00282 _Poly2Orient<3> toLocalCoords(const Point<3>& origin, const Quaternion& rotation) const
00283 {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(origin, rotation);
00284 p.m_axes[0].rotate(rotation.inverse());
00285 p.m_axes[0].rotate(rotation.inverse()); return p;}
00286
00287
00288
00289 Vector<dim> offset(const Point<dim>& pd, Point<2>& p2) const;
00290
00291
00292 bool checkContained(const Point<dim>& pd, Point<2> & p2) const;
00293
00294
00295
00296 bool checkIntersect(const AxisBox<dim>& b, Point<2>& p2, bool proper) const;
00297
00298 friend int _Intersect<dim>(const _Poly2Orient<dim> &, const _Poly2Orient<dim> &,
00299 _Poly2OrientIntersectData &);
00300
00301 private:
00302
00303 bool checkIntersectPlane(const AxisBox<dim>& b, Point<2>& p2, bool proper) const;
00304
00305 Point<dim> m_origin;
00306 Vector<dim> m_axes[2];
00307 };
00308
00310 template<const int dim>
00311 class Polygon
00312 {
00313 public:
00314 Polygon() {}
00315 Polygon(const Polygon& p) : m_orient(p.m_orient), m_poly(p.m_poly) {}
00316
00317 ~Polygon() {}
00318
00319 friend std::ostream& operator<< <dim>(std::ostream& os, const Polygon& p);
00320 friend std::istream& operator>> <dim>(std::istream& is, Polygon& p);
00321
00322 Polygon& operator=(const Polygon& p)
00323 {m_orient = p.m_orient; m_poly = p.m_poly; return *this;}
00324
00325 bool isEqualTo(const Polygon& p2, double epsilon = WFMATH_EPSILON) const;
00326
00327 bool operator==(const Polygon& p) const {return isEqualTo(p);}
00328 bool operator!=(const Polygon& p) const {return !isEqualTo(p);}
00329
00330 bool isValid() const {return m_poly.isValid();}
00331
00332
00333
00334 int numCorners() const {return m_poly.numCorners();}
00335 Point<dim> getCorner(int i) const
00336 {assert(i >= 0 && i < m_poly.numCorners()); return m_orient.convert(m_poly[i]);}
00337 Point<dim> getCenter() const {return m_orient.convert(m_poly.getCenter());}
00338
00339
00340
00341
00342
00343
00344 bool addCorner(int i, const Point<dim>& p, double epsilon = WFMATH_EPSILON);
00345
00346
00347 void removeCorner(int i);
00348
00349
00350
00351
00352
00353 bool moveCorner(int i, const Point<dim>& p, double epsilon = WFMATH_EPSILON);
00354
00355
00356 void clear() {m_poly.clear(); m_orient = _Poly2Orient<dim>();}
00357
00358
00359
00360 Polygon& shift(const Vector<dim>& v)
00361 {m_orient.shift(v); return *this;}
00362 Polygon& moveCornerTo(const Point<dim>& p, int corner)
00363 {return shift(p - getCorner(corner));}
00364 Polygon& moveCenterTo(const Point<dim>& p)
00365 {return shift(p - getCenter());}
00366
00367 Polygon& rotateCorner(const RotMatrix<dim>& m, int corner)
00368 {m_orient.rotate2(m, m_poly[corner]); return *this;}
00369 Polygon& rotateCenter(const RotMatrix<dim>& m)
00370 {if(m_poly.numCorners() > 0)
00371 m_orient.rotate2(m, m_poly.getCenter());
00372 return *this;}
00373 Polygon& rotatePoint(const RotMatrix<dim>& m, const Point<dim>& p)
00374 {m_orient.rotate(m, p); return *this;}
00375
00376
00377 Polygon<3>& rotateCorner(const Quaternion& q, int corner)
00378 {m_orient.rotate2(q, m_poly[corner]); return *this;}
00379 Polygon<3>& rotateCenter(const Quaternion& q)
00380 {if(m_poly.numCorners() > 0)
00381 m_orient.rotate2(q, m_poly.getCenter());
00382 return *this;}
00383 Polygon<3>& rotatePoint(const Quaternion& q, const Point<3>& p)
00384 {m_orient.rotate(q, p); return *this;}
00385
00386
00387
00388 AxisBox<dim> boundingBox() const;
00389 Ball<dim> boundingSphere() const;
00390 Ball<dim> boundingSphereSloppy() const;
00391
00392 Polygon toParentCoords(const Point<dim>& origin,
00393 const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00394 {Polygon p(*this); p.m_orient = m_orient.toParentCoords(origin, rotation); return p;}
00395 Polygon toParentCoords(const AxisBox<dim>& coords) const
00396 {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
00397 Polygon toParentCoords(const RotBox<dim>& coords) const
00398 {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
00399
00400
00401
00402
00403
00404 Polygon toLocalCoords(const Point<dim>& origin,
00405 const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
00406 {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(origin, rotation); return p;}
00407 Polygon toLocalCoords(const AxisBox<dim>& coords) const
00408 {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
00409 Polygon toLocalCoords(const RotBox<dim>& coords) const
00410 {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
00411
00412
00413 Polygon<3> toParentCoords(const Point<3>& origin, const Quaternion& rotation) const
00414 {Polygon<3> p(*this); p.m_orient = m_orient.toParentCoords(origin, rotation); return p;}
00415 Polygon<3> toLocalCoords(const Point<3>& origin, const Quaternion& rotation) const
00416 {Polygon<3> p(*this); p.m_orient = m_orient.toLocalCoords(origin, rotation); return p;}
00417
00418 friend bool Intersect<dim>(const Polygon& r, const Point<dim>& p, bool proper);
00419 friend bool Contains<dim>(const Point<dim>& p, const Polygon& r, bool proper);
00420
00421 friend bool Intersect<dim>(const Polygon& p, const AxisBox<dim>& b, bool proper);
00422 friend bool Contains<dim>(const Polygon& p, const AxisBox<dim>& b, bool proper);
00423 friend bool Contains<dim>(const AxisBox<dim>& b, const Polygon& p, bool proper);
00424
00425 friend bool Intersect<dim>(const Polygon& p, const Ball<dim>& b, bool proper);
00426 friend bool Contains<dim>(const Polygon& p, const Ball<dim>& b, bool proper);
00427 friend bool Contains<dim>(const Ball<dim>& b, const Polygon& p, bool proper);
00428
00429 friend bool Intersect<dim>(const Polygon& r, const Segment<dim>& s, bool proper);
00430 friend bool Contains<dim>(const Polygon& p, const Segment<dim>& s, bool proper);
00431 friend bool Contains<dim>(const Segment<dim>& s, const Polygon& p, bool proper);
00432
00433 friend bool Intersect<dim>(const Polygon& p, const RotBox<dim>& r, bool proper);
00434 friend bool Contains<dim>(const Polygon& p, const RotBox<dim>& r, bool proper);
00435 friend bool Contains<dim>(const RotBox<dim>& r, const Polygon& p, bool proper);
00436
00437 friend bool Intersect<dim>(const Polygon& p1, const Polygon& p2, bool proper);
00438 friend bool Contains<dim>(const Polygon& outer, const Polygon& inner, bool proper);
00439
00440 private:
00441
00442 _Poly2Orient<dim> m_orient;
00443 Polygon<2> m_poly;
00444 };
00445
00446 }
00447
00448 #include <wfmath/polygon_funcs.h>
00449
00450 #endif // WFMATH_POLYGON_H