WFMath  1.0.1
polygon.h
1 // polygon.h (A 2D polygon embeded in a <dim> dimensional space)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2002 The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 //
23 
24 // Author: Ron Steinke
25 
26 #ifndef WFMATH_POLYGON_H
27 #define WFMATH_POLYGON_H
28 
29 #include <wfmath/const.h>
30 #include <wfmath/axisbox.h>
31 #include <wfmath/ball.h>
32 #include <wfmath/quaternion.h>
33 
34 #include <vector>
35 
36 namespace WFMath {
37 
38 template<int dim> class Polygon;
39 
40 template<int dim>
41 std::ostream& operator<<(std::ostream& os, const Polygon<dim>& r);
42 template<int dim>
43 std::istream& operator>>(std::istream& is, Polygon<dim>& r);
44 
46 template<>
47 class Polygon<2>
48 {
49  public:
50  Polygon() : m_points() {}
51  Polygon(const Polygon& p) : m_points(p.m_points) {}
53  explicit Polygon(const AtlasInType& a) : m_points() {fromAtlas(a);}
54 
55  ~Polygon() {}
56 
57  friend std::ostream& operator<< <2>(std::ostream& os, const Polygon& p);
58  friend std::istream& operator>> <2>(std::istream& is, Polygon& p);
59 
60 
62  AtlasOutType toAtlas() const;
64  void fromAtlas(const AtlasInType& a);
65 
66  Polygon& operator=(const Polygon& p)
67  {m_points = p.m_points; return *this;}
68 
69  bool isEqualTo(const Polygon& p, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;
70 
71  bool operator==(const Polygon& p) const {return isEqualTo(p);}
72  bool operator!=(const Polygon& p) const {return !isEqualTo(p);}
73 
74  bool isValid() const;
75 
76  // Descriptive characteristics
77 
78  size_t numCorners() const {return m_points.size();}
79  Point<2> getCorner(size_t i) const {return m_points[i];}
80  Point<2> getCenter() const {return Barycenter(m_points);}
81 
82  // For a Polygon<2>, addCorner() and moveCorner() always succeed.
83  // The return values are present for the sake of a unified template
84  // interface, and the epsilon argument is ignored
85 
86  // Add before i'th corner, zero is beginning, numCorners() is end
87  bool addCorner(size_t i, const Point<2>& p, CoordType = numeric_constants<CoordType>::epsilon())
88  {m_points.insert(m_points.begin() + i, p); return true;}
89 
90  // Remove the i'th corner
91  void removeCorner(size_t i) {m_points.erase(m_points.begin() + i);}
92 
93  // Move the i'th corner to p
94  bool moveCorner(size_t i, const Point<2>& p, CoordType = numeric_constants<CoordType>::epsilon())
95  {m_points[i] = p; return true;}
96 
97  // Remove all points
98  void clear() {m_points.clear();}
99 
100  const Point<2>& operator[](size_t i) const {return m_points[i];}
101  Point<2>& operator[](size_t i) {return m_points[i];}
102 
103  void resize(std::vector<Point<2> >::size_type size) {m_points.resize(size);}
104 
105  // Movement functions
106 
107  Polygon& shift(const Vector<2>& v);
108  Polygon& moveCornerTo(const Point<2>& p, size_t corner)
109  {return shift(p - getCorner(corner));}
110  Polygon& moveCenterTo(const Point<2>& p)
111  {return shift(p - getCenter());}
112 
113  Polygon& rotateCorner(const RotMatrix<2>& m, size_t corner)
114  {rotatePoint(m, getCorner(corner)); return *this;}
115  Polygon& rotateCenter(const RotMatrix<2>& m)
116  {rotatePoint(m, getCenter()); return *this;}
117  Polygon& rotatePoint(const RotMatrix<2>& m, const Point<2>& p);
118 
119  // Intersection functions
120 
121  AxisBox<2> boundingBox() const {return BoundingBox(m_points);}
122  Ball<2> boundingSphere() const {return BoundingSphere(m_points);}
123  Ball<2> boundingSphereSloppy() const {return BoundingSphereSloppy(m_points);}
124 
125  Polygon toParentCoords(const Point<2>& origin,
126  const RotMatrix<2>& rotation = RotMatrix<2>().identity()) const;
127  Polygon toParentCoords(const AxisBox<2>& coords) const;
128  Polygon toParentCoords(const RotBox<2>& coords) const;
129 
130  // toLocal is just like toParent, expect we reverse the order of
131  // translation and rotation and use the opposite sense of the rotation
132  // matrix
133 
134  Polygon toLocalCoords(const Point<2>& origin,
135  const RotMatrix<2>& rotation = RotMatrix<2>().identity()) const;
136  Polygon toLocalCoords(const AxisBox<2>& coords) const;
137  Polygon toLocalCoords(const RotBox<2>& coords) const;
138 
139  friend bool Intersect<2>(const Polygon& r, const Point<2>& p, bool proper);
140  friend bool Contains<2>(const Point<2>& p, const Polygon& r, bool proper);
141 
142  friend bool Intersect<2>(const Polygon& p, const AxisBox<2>& b, bool proper);
143  friend bool Contains<2>(const Polygon& p, const AxisBox<2>& b, bool proper);
144  friend bool Contains<2>(const AxisBox<2>& b, const Polygon& p, bool proper);
145 
146  friend bool Intersect<2>(const Polygon& p, const Ball<2>& b, bool proper);
147  friend bool Contains<2>(const Polygon& p, const Ball<2>& b, bool proper);
148  friend bool Contains<2>(const Ball<2>& b, const Polygon& p, bool proper);
149 
150  friend bool Intersect<2>(const Polygon& r, const Segment<2>& s, bool proper);
151  friend bool Contains<2>(const Polygon& p, const Segment<2>& s, bool proper);
152  friend bool Contains<2>(const Segment<2>& s, const Polygon& p, bool proper);
153 
154  friend bool Intersect<2>(const Polygon& p, const RotBox<2>& r, bool proper);
155  friend bool Contains<2>(const Polygon& p, const RotBox<2>& r, bool proper);
156  friend bool Contains<2>(const RotBox<2>& r, const Polygon& p, bool proper);
157 
158  friend bool Intersect<2>(const Polygon& p1, const Polygon& p2, bool proper);
159  friend bool Contains<2>(const Polygon& outer, const Polygon& inner, bool proper);
160 
161 private:
162  std::vector<Point<2> > m_points;
163  typedef std::vector<Point<2> >::iterator theIter;
164  typedef std::vector<Point<2> >::const_iterator theConstIter;
165 
166 };
167 
168 // Helper classes, to keep track of the orientation of
169 // a 2D polygon in dim dimensions
170 
171 typedef enum {
172  _WFMATH_POLY2REORIENT_NONE,
173  _WFMATH_POLY2REORIENT_CLEAR_AXIS2,
174  _WFMATH_POLY2REORIENT_CLEAR_BOTH_AXES,
175  _WFMATH_POLY2REORIENT_MOVE_AXIS2_TO_AXIS1,
176  _WFMATH_POLY2REORIENT_SCALE1_CLEAR2
177 } _Poly2ReorientType;
178 
179 // Reorient a 2D polygon to match a change in the basis
180 // used by _Poly2Orient
181 class _Poly2Reorient
182 {
183 public:
184  _Poly2Reorient(_Poly2ReorientType type, CoordType scale = 0.0)
185  : m_type(type), m_scale(scale) {}
186  ~_Poly2Reorient() {}
187 
188  void reorient(Polygon<2>& poly, size_t skip = std::numeric_limits<size_t>::max()) const;
189 
190 private:
191  _Poly2ReorientType m_type;
192  CoordType m_scale;
193 };
194 
195 template<int dim> class _Poly2Orient;
196 
197 struct _Poly2OrientIntersectData {
198  int dim;
199  Point<2> p1, p2;
200  Vector<2> v1, v2, off;
201  bool o1_is_line, o2_is_line;
202 };
203 
204 // Finds the intersection of the two planes, returns the
205 // dimension of the intersection space, the rest of the arguments
206 // are various information returned depending on the dimension of
207 // the intersection
208 template<int dim>
209 int _Intersect(const _Poly2Orient<dim> &, const _Poly2Orient<dim> &,
210  _Poly2OrientIntersectData &);
211 
212 // Keep track of the orientation of a 2D polygon in dim dimensions
213 template<int dim>
214 class _Poly2Orient
215 {
216 public:
217  _Poly2Orient() : m_origin() {}
218  _Poly2Orient(const _Poly2Orient& p) : m_origin() {operator=(p);}
219  ~_Poly2Orient() {}
220 
221  _Poly2Orient& operator=(const _Poly2Orient& p);
222 
223  // Convert a point in the 2D polygon to a point in dim dimensional space
224  Point<dim> convert(const Point<2>& p) const;
225 
226  // Try to convert a point from dim dimensions into 2D, expanding the
227  // basis if necessary. Returns true on success. On failure, the
228  // basis is unchanged.
229  bool expand(const Point<dim>& pd, Point<2>& p2, CoordType epsilon = numeric_constants<CoordType>::epsilon());
230 
231  // Reduce the basis to the minimum necessary to span the points in
232  // poly (with the exception of skip). Returns _Poly2Reorient, which needs
233  // to be used to reorient the points to match the new basis.
234  _Poly2Reorient reduce(const Polygon<2>& poly, size_t skip = std::numeric_limits<size_t>::max());
235 
236  void shift(const Vector<dim>& v) {if(m_origin.isValid()) m_origin += v;}
237  void rotate(const RotMatrix<dim>& m, const Point<dim>& p);
238  // Rotates about the point which corresponds to "p" in the oriented plane
239  void rotate2(const RotMatrix<dim>& m, const Point<2>& p);
240 
241 //3D only
242  void rotate(const Quaternion& q, const Point<3>& p);
243  // Rotates about the point which corresponds to "p" in the oriented plane
244  void rotate2(const Quaternion& q, const Point<2>& p);
245 
246  _Poly2Orient toParentCoords(const Point<dim>& origin,
247  const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
248  {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(origin, rotation);
249  p.m_axes[0].rotate(rotation); p.m_axes[1].rotate(rotation); return p;}
250  _Poly2Orient toParentCoords(const AxisBox<dim>& coords) const
251  {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords); return p;}
252  _Poly2Orient toParentCoords(const RotBox<dim>& coords) const
253  {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords);
254  p.m_axes[0].rotate(coords.orientation());
255  p.m_axes[1].rotate(coords.orientation()); return p;}
256 
257  // toLocal is just like toParent, expect we reverse the order of
258  // translation and rotation and use the opposite sense of the rotation
259  // matrix
260 
261  _Poly2Orient toLocalCoords(const Point<dim>& origin,
262  const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
263  {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(origin, rotation);
264  p.m_axes[0] = rotation * p.m_axes[0];
265  p.m_axes[1] = rotation * p.m_axes[1]; return p;}
266  _Poly2Orient toLocalCoords(const AxisBox<dim>& coords) const
267  {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords); return p;}
268  _Poly2Orient toLocalCoords(const RotBox<dim>& coords) const
269  {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords);
270  p.m_axes[0] = coords.orientation() * p.m_axes[0];
271  p.m_axes[1] = coords.orientation() * p.m_axes[1]; return p;}
272 
273  // 3D only
274  _Poly2Orient<3> toParentCoords(const Point<3>& origin, const Quaternion& rotation) const
275  {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(origin, rotation);
276  p.m_axes[0].rotate(rotation); p.m_axes[0].rotate(rotation); return p;}
277  _Poly2Orient<3> toLocalCoords(const Point<3>& origin, const Quaternion& rotation) const
278  {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(origin, rotation);
279  p.m_axes[0].rotate(rotation.inverse());
280  p.m_axes[0].rotate(rotation.inverse()); return p;}
281 
282  // Gives the offset from pd to the space spanned by
283  // the basis, and puts the nearest point in p2.
284  Vector<dim> offset(const Point<dim>& pd, Point<2>& p2) const;
285 
286  // Like offset, but returns true if the point is in the plane
287  bool checkContained(const Point<dim>& pd, Point<2> & p2) const;
288 
289  // Check if the AxisBox intersects the spanned space, and if so
290  // return a point in the intersection.
291  bool checkIntersect(const AxisBox<dim>& b, Point<2>& p2, bool proper) const;
292 
293  friend int _Intersect<dim>(const _Poly2Orient<dim> &, const _Poly2Orient<dim> &,
294  _Poly2OrientIntersectData &);
295 
296 private:
297  // special case of the above when both axes are valid
298  bool checkIntersectPlane(const AxisBox<dim>& b, Point<2>& p2, bool proper) const;
299 
300  Point<dim> m_origin;
301  Vector<dim> m_axes[2]; // Normalized to unit length
302 };
303 
305 template<int dim = 3>
306 class Polygon
307 {
308 public:
309  Polygon() : m_orient(), m_poly() {}
310  Polygon(const Polygon& p) : m_orient(p.m_orient), m_poly(p.m_poly) {}
311 
312  ~Polygon() {}
313 
314  friend std::ostream& operator<< <dim>(std::ostream& os, const Polygon& p);
315  friend std::istream& operator>> <dim>(std::istream& is, Polygon& p);
316 
317  Polygon& operator=(const Polygon& p)
318  {m_orient = p.m_orient; m_poly = p.m_poly; return *this;}
319 
320  bool isEqualTo(const Polygon& p2, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;
321 
322  bool operator==(const Polygon& p) const {return isEqualTo(p);}
323  bool operator!=(const Polygon& p) const {return !isEqualTo(p);}
324 
325  bool isValid() const {return m_poly.isValid();}
326 
327  // Descriptive characteristics
328 
329  size_t numCorners() const {return m_poly.numCorners();}
330  Point<dim> getCorner(size_t i) const {return m_orient.convert(m_poly[i]);}
331  Point<dim> getCenter() const {return m_orient.convert(m_poly.getCenter());}
332 
333  // The failure of the following functions does not invalidate the
334  // polygon, but merely leaves it unchaged.
335 
336  // Add before i'th corner, zero is beginning, numCorners() is end
337  // Only succeeds if p lies in a plane with all current points
338  bool addCorner(size_t i, const Point<dim>& p, CoordType epsilon = numeric_constants<CoordType>::epsilon());
339 
340  // Remove the i'th corner
341  void removeCorner(size_t i);
342 
343  // Move the i'th corner to p, only succeeds if new location
344  // lies in the same plane as all the other points. Note that,
345  // under certain circumstances, this plane may not contain the
346  // original location of the point.
347  bool moveCorner(size_t i, const Point<dim>& p, CoordType epsilon = numeric_constants<CoordType>::epsilon());
348 
349  // Remove all points
350  void clear() {m_poly.clear(); m_orient = _Poly2Orient<dim>();}
351 
352  // Movement functions
353 
354  Polygon& shift(const Vector<dim>& v)
355  {m_orient.shift(v); return *this;}
356  Polygon& moveCornerTo(const Point<dim>& p, size_t corner)
357  {return shift(p - getCorner(corner));}
358  Polygon& moveCenterTo(const Point<dim>& p)
359  {return shift(p - getCenter());}
360 
361  Polygon& rotateCorner(const RotMatrix<dim>& m, size_t corner)
362  {m_orient.rotate2(m, m_poly[corner]); return *this;}
363  Polygon& rotateCenter(const RotMatrix<dim>& m)
364  {if(m_poly.numCorners() > 0)
365  m_orient.rotate2(m, m_poly.getCenter());
366  return *this;}
367  Polygon& rotatePoint(const RotMatrix<dim>& m, const Point<dim>& p)
368  {m_orient.rotate(m, p); return *this;}
369 
370  // 3D rotation functions
371  Polygon<3>& rotateCorner(const Quaternion& q, size_t corner)
372  {m_orient.rotate2(q, m_poly[corner]); return *this;}
373  Polygon<3>& rotateCenter(const Quaternion& q)
374  {if(m_poly.numCorners() > 0)
375  m_orient.rotate2(q, m_poly.getCenter());
376  return *this;}
377  Polygon<3>& rotatePoint(const Quaternion& q, const Point<3>& p)
378  {m_orient.rotate(q, p); return *this;}
379 
380  // Intersection functions
381 
382  AxisBox<dim> boundingBox() const;
383  Ball<dim> boundingSphere() const;
384  Ball<dim> boundingSphereSloppy() const;
385 
386  Polygon toParentCoords(const Point<dim>& origin,
387  const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
388  {Polygon p(*this); p.m_orient = m_orient.toParentCoords(origin, rotation); return p;}
389  Polygon toParentCoords(const AxisBox<dim>& coords) const
390  {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
391  Polygon toParentCoords(const RotBox<dim>& coords) const
392  {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
393 
394  // toLocal is just like toParent, expect we reverse the order of
395  // translation and rotation and use the opposite sense of the rotation
396  // matrix
397 
398  Polygon toLocalCoords(const Point<dim>& origin,
399  const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const
400  {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(origin, rotation); return p;}
401  Polygon toLocalCoords(const AxisBox<dim>& coords) const
402  {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
403  Polygon toLocalCoords(const RotBox<dim>& coords) const
404  {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
405 
406  // 3D only
407  Polygon<3> toParentCoords(const Point<3>& origin, const Quaternion& rotation) const
408  {Polygon<3> p(*this); p.m_orient = m_orient.toParentCoords(origin, rotation); return p;}
409  Polygon<3> toLocalCoords(const Point<3>& origin, const Quaternion& rotation) const
410  {Polygon<3> p(*this); p.m_orient = m_orient.toLocalCoords(origin, rotation); return p;}
411 
412  friend bool Intersect<dim>(const Polygon& r, const Point<dim>& p, bool proper);
413  friend bool Contains<dim>(const Point<dim>& p, const Polygon& r, bool proper);
414 
415  friend bool Intersect<dim>(const Polygon& p, const AxisBox<dim>& b, bool proper);
416  friend bool Contains<dim>(const Polygon& p, const AxisBox<dim>& b, bool proper);
417  friend bool Contains<dim>(const AxisBox<dim>& b, const Polygon& p, bool proper);
418 
419  friend bool Intersect<dim>(const Polygon& p, const Ball<dim>& b, bool proper);
420  friend bool Contains<dim>(const Polygon& p, const Ball<dim>& b, bool proper);
421  friend bool Contains<dim>(const Ball<dim>& b, const Polygon& p, bool proper);
422 
423  friend bool Intersect<dim>(const Polygon& r, const Segment<dim>& s, bool proper);
424  friend bool Contains<dim>(const Polygon& p, const Segment<dim>& s, bool proper);
425  friend bool Contains<dim>(const Segment<dim>& s, const Polygon& p, bool proper);
426 
427  friend bool Intersect<dim>(const Polygon& p, const RotBox<dim>& r, bool proper);
428  friend bool Contains<dim>(const Polygon& p, const RotBox<dim>& r, bool proper);
429  friend bool Contains<dim>(const RotBox<dim>& r, const Polygon& p, bool proper);
430 
431  friend bool Intersect<dim>(const Polygon& p1, const Polygon& p2, bool proper);
432  friend bool Contains<dim>(const Polygon& outer, const Polygon& inner, bool proper);
433 
434  private:
435 
436  _Poly2Orient<dim> m_orient;
437  Polygon<2> m_poly;
438 };
439 
440 template<int dim>
441 inline bool Polygon<dim>::addCorner(size_t i, const Point<dim>& p, CoordType epsilon)
442 {
443  Point<2> p2;
444  bool succ = m_orient.expand(p, p2, epsilon);
445  if(succ)
446  m_poly.addCorner(i, p2, epsilon);
447  return succ;
448 }
449 
450 template<int dim>
451 inline void Polygon<dim>::removeCorner(size_t i)
452 {
453  m_poly.removeCorner(i);
454  _Poly2Reorient r = m_orient.reduce(m_poly);
455  r.reorient(m_poly);
456 }
457 
458 template<int dim>
459 inline bool Polygon<dim>::moveCorner(size_t i, const Point<dim>& p, CoordType epsilon)
460 {
461  _Poly2Orient<dim> try_orient = m_orient;
462  _Poly2Reorient r = try_orient.reduce(m_poly, i);
463  Point<2> p2;
464 
465  if(!try_orient.expand(p, p2, epsilon))
466  return false;
467 
468  r.reorient(m_poly, i);
469  m_poly[i] = p2;
470  m_orient = try_orient;
471 
472  return true;
473 }
474 
475 } // namespace WFMath
476 
477 #endif // WFMATH_POLYGON_H