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