00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
#ifndef WFMATH_POLYGON_FUNCS_H
00027
#define WFMATH_POLYGON_FUNCS_H
00028
00029
#include <wfmath/const.h>
00030
#include <wfmath/vector.h>
00031
#include <wfmath/point.h>
00032
#include <wfmath/axisbox.h>
00033
#include <wfmath/ball.h>
00034
#include <wfmath/polygon.h>
00035
00036
namespace WFMath {
00037
00038
template<const
int dim>
00039
inline _Poly2Orient<dim>& _Poly2Orient<dim>::operator=(
const _Poly2Orient<dim>& a)
00040 {
00041 m_origin = a.m_origin;
00042
00043
for(
int i = 0; i < 2; ++i)
00044 m_axes[i] = a.m_axes[i];
00045
00046
return *
this;
00047 }
00048
00049
template<const
int dim>
00050
inline bool Polygon<dim>::isEqualTo(
const Polygon<dim>& p,
double epsilon)
const
00051
{
00052
00053
00054
00055
int size = m_poly.numCorners();
00056
if(size != p.m_poly.numCorners())
00057
return false;
00058
00059
for(
int i = 0; i < size; ++i)
00060
if(!
Equal(getCorner(i), p.getCorner(i), epsilon))
00061
return false;
00062
00063
return true;
00064 }
00065
00066
template<const
int dim>
00067
inline Point<dim> _Poly2Orient<dim>::convert(
const Point<2>& p)
const
00068
{
00069 assert(m_origin.isValid());
00070
00071 Point<dim> out = m_origin;
00072
00073
for(
int j = 0; j < 2; ++j) {
00074
if(m_axes[j].isValid())
00075 out += m_axes[j] * p[j];
00076
else
00077 assert(p[j] == 0);
00078 }
00079
00080 out.setValid(p.isValid());
00081
00082
return out;
00083 }
00084
00085
template<const
int dim>
00086
bool _Poly2Orient<dim>::expand(
const Point<dim>& pd, Point<2>& p2,
double epsilon)
00087 {
00088 p2[0] = p2[1] = 0;
00089 p2.setValid();
00090
00091
if(!m_origin.isValid()) {
00092 m_origin = pd;
00093 m_origin.setValid();
00094
return true;
00095 }
00096
00097 Vector<dim> shift = pd - m_origin, start_shift = shift;
00098
00099
CoordType bound = shift.sqrMag() * epsilon;
00100
00101
int j = 0;
00102
00103
while(
true) {
00104
if(Dot(shift, start_shift) <= bound)
00105
return true;
00106
00107
if(j == 2) {
00108 p2.setValid(
false);
00109
return false;
00110 }
00111
00112
if(!m_axes[j].isValid()) {
00113 p2[j] = shift.mag();
00114 m_axes[j] = shift / p2[j];
00115 m_axes[j].setValid();
00116
return true;
00117 }
00118
00119 p2[j] = Dot(shift, m_axes[j]);
00120 shift -= m_axes[j] * p2[j];
00121 ++j;
00122 }
00123 }
00124
00125
template<const
int dim>
00126 _Poly2Reorient _Poly2Orient<dim>::reduce(
const Polygon<2>& poly,
int skip)
00127 {
00128
if(poly.numCorners() <= ((skip == 0) ? 1 : 0)) {
00129 m_origin.setValid(
false);
00130 m_axes[0].setValid(
false);
00131 m_axes[1].setValid(
false);
00132
return _WFMATH_POLY2REORIENT_NONE;
00133 }
00134
00135 assert(m_origin.isValid());
00136
00137
if(!m_axes[0].isValid())
00138
return _WFMATH_POLY2REORIENT_NONE;
00139
00140
00141
00142
bool still_valid[2] = {
false,}, got_ratio =
false;
00143
CoordType ratio, size;
00144
double epsilon;
00145
int i, end = poly.numCorners();
00146
00147
00148
for(i = 0; i < end; ++i) {
00149
if(i == skip)
00150
continue;
00151
const Point<2> &p = poly[i];
00152
CoordType x = fabs(p[0]), y = fabs(p[1]), max = (x > y) ? x : y;
00153
if(i = 0 || max < size)
00154 size = max;
00155 }
00156
CoordType exponent;
00157 (
void) frexp(size, exponent);
00158 epsilon = ldexp(WFMATH_EPSILON, exponent);
00159
00160 i = 0;
00161
if(skip == 0)
00162 ++i;
00163 assert(i != end);
00164 Point<2> first_point = poly[i];
00165 first_point.setValid();
00166
00167
while(++i != end) {
00168
if(i == skip)
00169
continue;
00170
00171 Vector<2> diff = m_poly[i] - first_point;
00172
if(diff.sqrMag() < epsilon * epsilon)
00173
continue;
00174
if(!m_axes[1].isValid())
00175
return _WFMATH_POLY2REORIENT_NONE;
00176
for(
int j = 0; j < 2; ++j) {
00177
if(fabs(diff[j]) < epsilon) {
00178 assert(diff[j ? 0 : 1] >= epsilon);
00179
if(still_valid[j ? 0 : 1] || got_ratio)
00180
return _WFMATH_POLY2REORIENT_NONE;
00181 still_valid[j] =
true;
00182 }
00183 }
00184
00185
if(still_valid[0] || still_valid[1])
00186
return _WFMATH_POLY2REORIENT_NONE;
00187
CoordType new_ratio = diff[1] / diff[0];
00188
if(!got_ratio) {
00189 ratio = new_ratio;
00190 got_ratio =
true;
00191
continue;
00192 }
00193
if(!
Equal(ratio, new_ratio))
00194
return _WFMATH_POLY2REORIENT_NONE;
00195 }
00196
00197
00198
00199
if(still_valid[0]) {
00200 assert(m_axes[1].isValid());
00201 assert(!still_valid[1]);
00202 assert(!got_ratio);
00203
00204 m_origin += m_axes[1] * first_point[1];
00205 m_axes[1].setValid(
false);
00206
return _WFMATH_POLY2REORIENT_CLEAR_AXIS2;
00207 }
00208
00209
if(still_valid[1]) {
00210 assert(m_axes_valid[1]);
00211 assert(!got_ratio);
00212
00213 m_origin += m_axes[0] * first_point[0];
00214 m_axes[0] = m_axes[1];
00215 m_axes[1].setValid(
false);
00216
return _WFMATH_POLY2REORIENT_MOVE_AXIS2_TO_AXIS1;
00217 }
00218
00219
00220
if(!got_ratio) {
00221 m_origin += m_axes[0] * first_point[0];
00222
if(m_axes_valid[1])
00223 m_origin += m_axes[1] * first_point[1];
00224 m_axes[0].setValid(
false);
00225 m_axes[1].setValid(
false);
00226
return _WFMATH_POLY2REORIENT_CLEAR_BOTH_AXES;
00227 }
00228
00229 assert(m_axes[1].isValid());
00230
00231
00232
00233
00234 Vector<dim> new0;
00235 new0 = m_axes[0] + m_axes[1] * ratio;
00236
CoordType norm = new0.mag();
00237 new0 /= norm;
00238
00239
00240
00241
00242
00243
00244
00245 m_origin += m_axes[1] * (first_point[1] - ratio * first_point[0]);
00246
00247 m_axes[0] = new0;
00248 m_axes[1].setValid(
false);
00249
return _Poly2Reorient(_WFMATH_POLY2REORIENT_SCALE1_CLEAR2, norm);
00250 }
00251
00252
template<const
int dim>
00253
inline void _Poly2Orient<dim>::rotate(
const RotMatrix<dim>& m,
const Point<dim>& p)
00254 {
00255 m_origin.rotate(m, p);
00256
00257
for(
int j = 0; j < 2; ++j)
00258 m_axes[j] =
Prod(m_axes[j], m);
00259 }
00260
00261
template<const
int dim>
00262
void _Poly2Orient<dim>::rotate2(
const RotMatrix<dim>& m,
const Point<2>& p)
00263 {
00264 assert(m_origin.isValid());
00265
00266
if(!m_axes[0].isValid()) {
00267 assert(p[0] == 0 && p[1] == 0);
00268
return;
00269 }
00270
00271 Vector<dim> shift = m_axes[0] * p[0];
00272 m_axes[0] =
Prod(m_axes[0], m);
00273
00274
if(m_axes[1].isValid()) {
00275 shift += m_axes[1] * p[1];
00276 m_axes[1] =
Prod(m_axes[1], m);
00277 }
00278
else
00279 assert(p[1] == 0);
00280
00281 m_origin += shift -
Prod(shift, m);
00282 }
00283
00284
template<>
00285
inline void _Poly2Orient<3>::rotate(
const Quaternion& q,
const Point<3>& p)
00286 {
00287 m_origin.rotate(q, p);
00288
00289
for(
int j = 0; j < 2; ++j)
00290 m_axes[j].rotate(q);
00291 }
00292
00293
template<>
00294
inline void _Poly2Orient<3>::rotate2(
const Quaternion& q,
const Point<2>& p)
00295 {
00296 assert(m_origin.isValid());
00297
00298
if(!m_axes[0].isValid()) {
00299 assert(p[0] == 0 && p[1] == 0);
00300
return;
00301 }
00302
00303 Vector<3> shift = m_axes[0] * p[0];
00304 m_axes[0].rotate(q);
00305
00306
if(m_axes[1].isValid()) {
00307 shift += m_axes[1] * p[1];
00308 m_axes[1].rotate(q);
00309 }
00310
else
00311 assert(p[1] == 0);
00312
00313 m_origin += shift - shift.rotate(q);
00314 }
00315
00316
template<const
int dim>
00317
inline bool Polygon<dim>::addCorner(
int i,
const Point<dim>& p,
double epsilon)
00318 {
00319 Point<2> p2;
00320
bool succ = m_orient.expand(p, p2, epsilon);
00321
if(succ)
00322 m_poly.addCorner(i, p2, epsilon);
00323
return succ;
00324 }
00325
00326
template<const
int dim>
00327
inline void Polygon<dim>::removeCorner(
int i)
00328 {
00329 m_poly.removeCorner(i);
00330 _Poly2Reorient r = m_orient.reduce(m_poly);
00331 r.reorient(m_poly);
00332 }
00333
00334
template<const
int dim>
00335
inline bool Polygon<dim>::moveCorner(
int i,
const Point<dim>& p,
double epsilon)
00336 {
00337 _Poly2Orient<dim> try_orient = m_orient;
00338 _Poly2Reorient r = try_orient.reduce(m_poly, i);
00339 Point<2> p2;
00340
00341
if(!try_orient.expand(p, p2, epsilon))
00342
return false;
00343
00344 r.reorient(m_poly, i);
00345 m_poly[i] = p2;
00346 m_orient = try_orient;
00347
00348
return true;
00349 }
00350
00351
template<const
int dim>
00352 AxisBox<dim> Polygon<dim>::boundingBox()
const
00353
{
00354 assert(m_poly.numCorners() > 0);
00355
00356 Point<dim> min = m_orient.convert(m_poly[0]), max = min;
00357
bool valid = min.isValid();
00358
00359
for(
int i = 1; i != m_poly.numCorners(); ++i) {
00360 Point<dim> p = m_orient.convert(m_poly[i]);
00361 valid = valid && p.isValid();
00362
for(
int j = 0; j < dim; ++j) {
00363
if(p[j] < min[j])
00364 min[j] = p[j];
00365
if(p[j] > max[j])
00366 max[j] = p[j];
00367 }
00368 }
00369
00370 min.setValid(valid);
00371 max.setValid(valid);
00372
00373
return AxisBox<dim>(min, max,
true);
00374 }
00375
00376
template<const
int dim>
00377
inline Ball<dim> Polygon<dim>::boundingSphere()
const
00378
{
00379 Ball<2> b = m_poly.boundingSphere();
00380
00381
return Ball<dim>(m_orient.convert(b.center()), b.radius());
00382 }
00383
00384
template<const
int dim>
00385
inline Ball<dim> Polygon<dim>::boundingSphereSloppy()
const
00386
{
00387 Ball<2> b = m_poly.boundingSphereSloppy();
00388
00389
return Ball<dim>(m_orient.convert(b.center()), b.radius());
00390 }
00391
00392 }
00393
00394
#endif // WFMATH_POLYGON_FUNCS_H