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 int 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 = 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[1].isValid());
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[1].isValid())
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