1 // polygon_funcs.h (line polygon implementation)
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_FUNCS_H
27 #define WFMATH_POLYGON_FUNCS_H
28
29 #include <wfmath/polygon.h>
30
31 #include <wfmath/vector.h>
32 #include <wfmath/point.h>
33 #include <wfmath/ball.h>
34
35 #include <cmath>
36
37 #include <cassert>
38 #include <limits>
39
40 namespace WFMath {
41
42 template<int dim>
43 inline _Poly2Orient<dim>& _Poly2Orient<dim>::operator=(const _Poly2Orient<dim>& a)
44 {
45 m_origin = a.m_origin;
46
47 for(int i = 0; i < 2; ++i)
48 m_axes[i] = a.m_axes[i];
49
50 return *this;
51 }
52
53 template<int dim>
isEqualTo(const Polygon<dim> & p,CoordType epsilon)54 inline bool Polygon<dim>::isEqualTo(const Polygon<dim>& p, CoordType epsilon) const
55 {
56 // The same polygon can be expressed in different ways in the interal
57 // format, so we have to call getCorner();
58
59 size_t size = m_poly.numCorners();
60 if(size != p.m_poly.numCorners())
61 return false;
62
63 for(size_t i = 0; i < size; ++i)
64 if(!Equal(getCorner(i), p.getCorner(i), epsilon))
65 return false;
66
67 return true;
68 }
69
70 template<int dim>
convert(const Point<2> & p)71 inline Point<dim> _Poly2Orient<dim>::convert(const Point<2>& p) const
72 {
73 assert(m_origin.isValid());
74
75 Point<dim> out = m_origin;
76
77 for(int j = 0; j < 2; ++j) {
78 if(m_axes[j].isValid())
79 out += m_axes[j] * p[j];
80 else
81 assert(p[j] == 0);
82 }
83
84 out.setValid(p.isValid());
85
86 return out;
87 }
88
89 template<int dim>
expand(const Point<dim> & pd,Point<2> & p2,CoordType epsilon)90 bool _Poly2Orient<dim>::expand(const Point<dim>& pd, Point<2>& p2, CoordType epsilon)
91 {
92 p2[0] = p2[1] = 0; // initialize
93 p2.setValid();
94
95 if(!m_origin.isValid()) { // Adding to an empty list
96 m_origin = pd;
97 m_origin.setValid();
98 return true;
99 }
100
101 Vector<dim> shift = pd - m_origin, start_shift = shift;
102
103 CoordType bound = shift.sqrMag() * epsilon;
104
105 int j = 0;
106
107 while(true) {
108 if(Dot(shift, start_shift) <= bound) // shift is effectively zero
109 return true;
110
111 if(j == 2) { // Have two axes, shift doesn't lie in their plane
112 p2.setValid(false);
113 return false;
114 }
115
116 if(!m_axes[j].isValid()) { // Didn't span this previously, now we do
117 p2[j] = shift.mag();
118 m_axes[j] = shift / p2[j];
119 m_axes[j].setValid();
120 return true;
121 }
122
123 p2[j] = Dot(shift, m_axes[j]);
124 shift -= m_axes[j] * p2[j]; // shift is now perpendicular to m_axes[j]
125 ++j;
126 }
127 }
128
129 template<int dim>
reduce(const Polygon<2> & poly,size_t skip)130 _Poly2Reorient _Poly2Orient<dim>::reduce(const Polygon<2>& poly, size_t skip)
131 {
132 if(poly.numCorners() <= ((skip == 0) ? 1 : 0)) { // No corners left
133 m_origin.setValid(false);
134 m_axes[0].setValid(false);
135 m_axes[1].setValid(false);
136 return _WFMATH_POLY2REORIENT_NONE;
137 }
138
139 assert(m_origin.isValid());
140
141 if(!m_axes[0].isValid())
142 return _WFMATH_POLY2REORIENT_NONE;
143
144 // Check that we still span both axes
145
146 bool still_valid[2] = {false,}, got_ratio = false;
147 CoordType ratio = std::numeric_limits<CoordType>::max();
148 CoordType size = std::numeric_limits<CoordType>::max();
149 CoordType epsilon;
150 size_t i, end = poly.numCorners();
151
152 // scale epsilon
153 for(i = 0; i < end; ++i) {
154 if(i == skip)
155 continue;
156 const Point<2> &p = poly[i];
157 CoordType x = std::fabs(p[0]),
158 y = std::fabs(p[1]),
159 max = (x > y) ? x : y;
160 if(i == 0 || max < size)
161 size = max;
162 }
163 int exponent;
164 (void) std::frexp(size, &exponent);
165 epsilon = std::ldexp(numeric_constants<CoordType>::epsilon(), exponent);
166
167 i = 0;
168 if(skip == 0)
169 ++i;
170 assert(i != end);
171 Point<2> first_point = poly[i];
172 first_point.setValid(); // in case someone stuck invalid points in the poly
173
174 while(++i != end) {
175 if(i == skip)
176 continue;
177
178 Vector<2> diff = poly[i] - first_point;
179 if(diff.sqrMag() < epsilon * epsilon) // No addition to span
180 continue;
181 if(!m_axes[1].isValid()) // We span 1D
182 return _WFMATH_POLY2REORIENT_NONE;
183 for(int j = 0; j < 2; ++j) {
184 if(std::fabs(diff[j]) < epsilon) {
185 assert(diff[j ? 0 : 1] >= epsilon); // because diff != 0
186 if(still_valid[j ? 0 : 1] || got_ratio) // We span a 2D space
187 return _WFMATH_POLY2REORIENT_NONE;
188 still_valid[j] = true;
189 }
190 }
191 // The point has both elements nonzero
192 if(still_valid[0] || still_valid[1]) // We span a 2D space
193 return _WFMATH_POLY2REORIENT_NONE;
194 CoordType new_ratio = diff[1] / diff[0];
195 if(!got_ratio) {
196 ratio = new_ratio;
197 got_ratio = true;
198 continue;
199 }
200 if(!Equal(ratio, new_ratio)) // We span a 2D space
201 return _WFMATH_POLY2REORIENT_NONE;
202 }
203
204 // Okay, we don't span both vectors. What now?
205
206 if(still_valid[0]) {
207 assert(m_axes[1].isValid());
208 assert(!still_valid[1]);
209 assert(!got_ratio);
210 // This is easy, m_axes[1] is just invalid
211 m_origin += m_axes[1] * first_point[1];
212 m_axes[1].setValid(false);
213 return _WFMATH_POLY2REORIENT_CLEAR_AXIS2;
214 }
215
216 if(still_valid[1]) {
217 assert(m_axes[1].isValid());
218 assert(!got_ratio);
219 // This is a little harder, m_axes[0] is invalid, must swap axes
220 m_origin += m_axes[0] * first_point[0];
221 m_axes[0] = m_axes[1];
222 m_axes[1].setValid(false);
223 return _WFMATH_POLY2REORIENT_MOVE_AXIS2_TO_AXIS1;
224 }
225
226 // The !m_axes[1].isValid() case reducing to a point falls into here
227 if(!got_ratio) { // Nothing's valid, all points are equal
228 m_origin += m_axes[0] * first_point[0];
229 if(m_axes[1].isValid())
230 m_origin += m_axes[1] * first_point[1];
231 m_axes[0].setValid(false);
232 m_axes[1].setValid(false);
233 return _WFMATH_POLY2REORIENT_CLEAR_BOTH_AXES;
234 }
235
236 assert(m_axes[1].isValid());
237
238 // All the points are colinear, along some line which is not parallel
239 // to either of the original axes
240
241 Vector<dim> new0;
242 new0 = m_axes[0] + m_axes[1] * ratio;
243 CoordType norm = new0.mag();
244 new0 /= norm;
245
246 // Vector diff = m_axes[0] * first_point[0] + m_axes[1] * first_point[1];
247 // // Causes Dot(diff, m_axes[0]) to vanish, so the point on the line
248 // // with x coordinate zero is the new origin
249 // diff -= new0 * (first_point[0] * norm);
250 // m_origin += diff;
251 // or, equivalently
252 m_origin += m_axes[1] * (first_point[1] - ratio * first_point[0]);
253
254 m_axes[0] = new0;
255 m_axes[1].setValid(false);
256 return _Poly2Reorient(_WFMATH_POLY2REORIENT_SCALE1_CLEAR2, norm);
257 }
258
259 template<int dim>
rotate(const RotMatrix<dim> & m,const Point<dim> & p)260 inline void _Poly2Orient<dim>::rotate(const RotMatrix<dim>& m, const Point<dim>& p)
261 {
262 m_origin.rotate(m, p);
263
264 for(int j = 0; j < 2; ++j)
265 m_axes[j] = Prod(m_axes[j], m);
266 }
267
268 template<int dim>
rotate2(const RotMatrix<dim> & m,const Point<2> & p)269 void _Poly2Orient<dim>::rotate2(const RotMatrix<dim>& m, const Point<2>& p)
270 {
271 assert(m_origin.isValid());
272
273 if(!m_axes[0].isValid()) {
274 assert(p[0] == 0 && p[1] == 0);
275 return;
276 }
277
278 Vector<dim> shift = m_axes[0] * p[0];
279 m_axes[0] = Prod(m_axes[0], m);
280
281 if(m_axes[1].isValid()) {
282 shift += m_axes[1] * p[1];
283 m_axes[1] = Prod(m_axes[1], m);
284 }
285 else
286 assert(p[1] == 0);
287
288 m_origin += shift - Prod(shift, m);
289 }
290
291 template<>
rotate(const Quaternion & q,const Point<3> & p)292 inline void _Poly2Orient<3>::rotate(const Quaternion& q, const Point<3>& p)
293 {
294 m_origin.rotate(q, p);
295
296 for(int j = 0; j < 2; ++j)
297 m_axes[j].rotate(q);
298 }
299
300 template<>
rotate2(const Quaternion & q,const Point<2> & p)301 inline void _Poly2Orient<3>::rotate2(const Quaternion& q, const Point<2>& p)
302 {
303 assert(m_origin.isValid());
304
305 if(!m_axes[0].isValid()) {
306 assert(p[0] == 0 && p[1] == 0);
307 return;
308 }
309
310 Vector<3> shift = m_axes[0] * p[0];
311 m_axes[0].rotate(q);
312
313 if(m_axes[1].isValid()) {
314 shift += m_axes[1] * p[1];
315 m_axes[1].rotate(q);
316 }
317 else
318 assert(p[1] == 0);
319
320 m_origin += shift - shift.rotate(q);
321 }
322
323 template<int dim>
boundingBox()324 AxisBox<dim> Polygon<dim>::boundingBox() const
325 {
326 assert(m_poly.numCorners() > 0);
327
328 Point<dim> min = m_orient.convert(m_poly[0]), max = min;
329 bool valid = min.isValid();
330
331 for(size_t i = 1; i != m_poly.numCorners(); ++i) {
332 Point<dim> p = m_orient.convert(m_poly[i]);
333 valid = valid && p.isValid();
334 for(int j = 0; j < dim; ++j) {
335 if(p[j] < min[j])
336 min[j] = p[j];
337 if(p[j] > max[j])
338 max[j] = p[j];
339 }
340 }
341
342 min.setValid(valid);
343 max.setValid(valid);
344
345 return AxisBox<dim>(min, max, true);
346 }
347
348 template<int dim>
boundingSphere()349 inline Ball<dim> Polygon<dim>::boundingSphere() const
350 {
351 Ball<2> b = m_poly.boundingSphere();
352
353 return Ball<dim>(m_orient.convert(b.center()), b.radius());
354 }
355
356 template<int dim>
boundingSphereSloppy()357 inline Ball<dim> Polygon<dim>::boundingSphereSloppy() const
358 {
359 Ball<2> b = m_poly.boundingSphereSloppy();
360
361 return Ball<dim>(m_orient.convert(b.center()), b.radius());
362 }
363
364 } // namespace WFMath
365
366 #endif // WFMATH_POLYGON_FUNCS_H
367