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
40namespace WFMath {
41
42template<int dim>
43inline 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
59template<int dim>
60inline 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
78template<int dim>
79bool 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
118template<int dim>
119Poly2Reorient 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
248template<int dim>
249inline 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
257template<int dim>
258void 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
280template<>
281inline 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
289template<>
290inline 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
312template<int dim>
313AxisBox<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
337template<int dim>
338inline 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
345template<int dim>
346inline 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
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
static FloatType epsilon()
This is the attempted precision of the library.