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
45 /// The 2D specialization of the Polygon<> template
46 template<>
47 class Polygon<2>
48 {
49 public:
Polygon()50 Polygon() : m_points() {}
Polygon(const Polygon & p)51 Polygon(const Polygon& p) : m_points(p.m_points) {}
52 /// Construct a polygon from an object passed by Atlas
Polygon(const AtlasInType & a)53 explicit Polygon(const AtlasInType& a) : m_points() {fromAtlas(a);}
54
~Polygon()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
61 /// Create an Atlas object from the box
62 AtlasOutType toAtlas() const;
63 /// Set the box's value to that given by an Atlas object
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
numCorners()78 size_t numCorners() const {return m_points.size();}
getCorner(size_t i)79 Point<2> getCorner(size_t i) const {return m_points[i];}
getCenter()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
removeCorner(size_t i)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
clear()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
resize(std::vector<Point<2>>::size_type size)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);
moveCornerTo(const Point<2> & p,size_t corner)108 Polygon& moveCornerTo(const Point<2>& p, size_t corner)
109 {return shift(p - getCorner(corner));}
moveCenterTo(const Point<2> & p)110 Polygon& moveCenterTo(const Point<2>& p)
111 {return shift(p - getCenter());}
112
rotateCorner(const RotMatrix<2> & m,size_t corner)113 Polygon& rotateCorner(const RotMatrix<2>& m, size_t corner)
114 {rotatePoint(m, getCorner(corner)); return *this;}
rotateCenter(const RotMatrix<2> & m)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
boundingBox()121 AxisBox<2> boundingBox() const {return BoundingBox(m_points);}
boundingSphere()122 Ball<2> boundingSphere() const {return BoundingSphere(m_points);}
boundingSphereSloppy()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)
m_type(type)185 : m_type(type), m_scale(scale) {}
~_Poly2Reorient()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:
_Poly2Orient()217 _Poly2Orient() : m_origin() {}
_Poly2Orient(const _Poly2Orient & p)218 _Poly2Orient(const _Poly2Orient& p) : m_origin() {operator=(p);}
~_Poly2Orient()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
shift(const Vector<dim> & v)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;}
toParentCoords(const AxisBox<dim> & coords)250 _Poly2Orient toParentCoords(const AxisBox<dim>& coords) const
251 {_Poly2Orient p(*this); p.m_origin = m_origin.toParentCoords(coords); return p;}
toParentCoords(const RotBox<dim> & coords)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;}
toLocalCoords(const AxisBox<dim> & coords)266 _Poly2Orient toLocalCoords(const AxisBox<dim>& coords) const
267 {_Poly2Orient p(*this); p.m_origin = m_origin.toLocalCoords(coords); return p;}
toLocalCoords(const RotBox<dim> & coords)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
toParentCoords(const Point<3> & origin,const Quaternion & rotation)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;}
toLocalCoords(const Point<3> & origin,const Quaternion & rotation)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
304 /// A polygon, all of whose points lie in a plane, embedded in dim dimensions
305 template<int dim = 3>
306 class Polygon
307 {
308 public:
Polygon()309 Polygon() : m_orient(), m_poly() {}
Polygon(const Polygon & p)310 Polygon(const Polygon& p) : m_orient(p.m_orient), m_poly(p.m_poly) {}
311
~Polygon()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
isValid()325 bool isValid() const {return m_poly.isValid();}
326
327 // Descriptive characteristics
328
numCorners()329 size_t numCorners() const {return m_poly.numCorners();}
getCorner(size_t i)330 Point<dim> getCorner(size_t i) const {return m_orient.convert(m_poly[i]);}
getCenter()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
clear()350 void clear() {m_poly.clear(); m_orient = _Poly2Orient<dim>();}
351
352 // Movement functions
353
shift(const Vector<dim> & v)354 Polygon& shift(const Vector<dim>& v)
355 {m_orient.shift(v); return *this;}
moveCornerTo(const Point<dim> & p,size_t corner)356 Polygon& moveCornerTo(const Point<dim>& p, size_t corner)
357 {return shift(p - getCorner(corner));}
moveCenterTo(const Point<dim> & p)358 Polygon& moveCenterTo(const Point<dim>& p)
359 {return shift(p - getCenter());}
360
rotateCorner(const RotMatrix<dim> & m,size_t corner)361 Polygon& rotateCorner(const RotMatrix<dim>& m, size_t corner)
362 {m_orient.rotate2(m, m_poly[corner]); return *this;}
rotateCenter(const RotMatrix<dim> & m)363 Polygon& rotateCenter(const RotMatrix<dim>& m)
364 {if(m_poly.numCorners() > 0)
365 m_orient.rotate2(m, m_poly.getCenter());
366 return *this;}
rotatePoint(const RotMatrix<dim> & m,const Point<dim> & p)367 Polygon& rotatePoint(const RotMatrix<dim>& m, const Point<dim>& p)
368 {m_orient.rotate(m, p); return *this;}
369
370 // 3D rotation functions
rotateCorner(const Quaternion & q,size_t corner)371 Polygon<3>& rotateCorner(const Quaternion& q, size_t corner)
372 {m_orient.rotate2(q, m_poly[corner]); return *this;}
rotateCenter(const Quaternion & q)373 Polygon<3>& rotateCenter(const Quaternion& q)
374 {if(m_poly.numCorners() > 0)
375 m_orient.rotate2(q, m_poly.getCenter());
376 return *this;}
rotatePoint(const Quaternion & q,const Point<3> & p)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;}
toParentCoords(const AxisBox<dim> & coords)389 Polygon toParentCoords(const AxisBox<dim>& coords) const
390 {Polygon p(*this); p.m_orient = m_orient.toParentCoords(coords); return p;}
toParentCoords(const RotBox<dim> & coords)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;}
toLocalCoords(const AxisBox<dim> & coords)401 Polygon toLocalCoords(const AxisBox<dim>& coords) const
402 {Polygon p(*this); p.m_orient = m_orient.toLocalCoords(coords); return p;}
toLocalCoords(const RotBox<dim> & coords)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
toParentCoords(const Point<3> & origin,const Quaternion & rotation)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;}
toLocalCoords(const Point<3> & origin,const Quaternion & rotation)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>
addCorner(size_t i,const Point<dim> & p,CoordType epsilon)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>
removeCorner(size_t i)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>
moveCorner(size_t i,const Point<dim> & p,CoordType epsilon)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
478