WFMath  1.0.1
polygon_funcs.h
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>
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>
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>
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>
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>
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>
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<>
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<>
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>
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>
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>
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