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