WFMath 0.3.11

polygon_funcs.h

00001 // polygon_funcs.h (line polygon implementation)
00002 //
00003 //  The WorldForge Project
00004 //  Copyright (C) 2002  The WorldForge Project
00005 //
00006 //  This program is free software; you can redistribute it and/or modify
00007 //  it under the terms of the GNU General Public License as published by
00008 //  the Free Software Foundation; either version 2 of the License, or
00009 //  (at your option) any later version.
00010 //
00011 //  This program is distributed in the hope that it will be useful,
00012 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 //  GNU General Public License for more details.
00015 //
00016 //  You should have received a copy of the GNU General Public License
00017 //  along with this program; if not, write to the Free Software
00018 //  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00019 //
00020 //  For information about WorldForge and its authors, please contact
00021 //  the Worldforge Web Site at http://www.worldforge.org.
00022 //
00023 
00024 // Author: Ron Steinke
00025 
00026 #ifndef WFMATH_POLYGON_FUNCS_H
00027 #define WFMATH_POLYGON_FUNCS_H
00028 
00029 #include <wfmath/polygon.h>
00030 
00031 #include <wfmath/vector.h>
00032 #include <wfmath/point.h>
00033 #include <wfmath/ball.h>
00034 
00035 #include <cmath>
00036 
00037 #include <cassert>
00038 
00039 namespace WFMath {
00040 
00041 template<const int dim>
00042 inline _Poly2Orient<dim>& _Poly2Orient<dim>::operator=(const _Poly2Orient<dim>& a)
00043 {
00044   m_origin = a.m_origin;
00045 
00046   for(int i = 0; i < 2; ++i)
00047     m_axes[i] = a.m_axes[i];
00048 
00049   return *this;
00050 }
00051 
00052 template<const int dim>
00053 inline bool Polygon<dim>::isEqualTo(const Polygon<dim>& p, double epsilon) const
00054 {
00055   // The same polygon can be expressed in different ways in the interal
00056   // format, so we have to call getCorner();
00057 
00058   int size = m_poly.numCorners();
00059   if(size != p.m_poly.numCorners())
00060     return false;
00061 
00062   for(int i = 0; i < size; ++i)
00063     if(!Equal(getCorner(i), p.getCorner(i), epsilon))
00064       return false;
00065 
00066   return true;
00067 }
00068 
00069 template<const int dim>
00070 inline Point<dim> _Poly2Orient<dim>::convert(const Point<2>& p) const
00071 {
00072   assert(m_origin.isValid());
00073 
00074   Point<dim> out = m_origin;
00075 
00076   for(int j = 0; j < 2; ++j) {
00077     if(m_axes[j].isValid())
00078       out += m_axes[j] * p[j];
00079     else
00080       assert(p[j] == 0);
00081   }
00082 
00083   out.setValid(p.isValid());
00084 
00085   return out;
00086 }
00087 
00088 template<const int dim>
00089 bool _Poly2Orient<dim>::expand(const Point<dim>& pd, Point<2>& p2, double epsilon)
00090 {
00091   p2[0] = p2[1] = 0; // initialize
00092   p2.setValid();
00093 
00094   if(!m_origin.isValid()) { // Adding to an empty list
00095     m_origin = pd;
00096     m_origin.setValid();
00097     return true;
00098   }
00099 
00100   Vector<dim> shift = pd - m_origin, start_shift = shift;
00101 
00102   CoordType bound = shift.sqrMag() * epsilon;
00103 
00104   int j = 0;
00105 
00106   while(true) {
00107     if(Dot(shift, start_shift) <= bound) // shift is effectively zero
00108       return true;
00109 
00110     if(j == 2) { // Have two axes, shift doesn't lie in their plane
00111       p2.setValid(false);
00112       return false;
00113     }
00114 
00115     if(!m_axes[j].isValid()) { // Didn't span this previously, now we do
00116       p2[j] = shift.mag();
00117       m_axes[j] = shift / p2[j];
00118       m_axes[j].setValid();
00119       return true;
00120    }
00121 
00122    p2[j] = Dot(shift, m_axes[j]);
00123    shift -= m_axes[j] * p2[j]; // shift is now perpendicular to m_axes[j]
00124    ++j;
00125   }
00126 }
00127 
00128 template<const int dim>
00129 _Poly2Reorient _Poly2Orient<dim>::reduce(const Polygon<2>& poly, int skip)
00130 {
00131   if(poly.numCorners() <= ((skip == 0) ? 1 : 0)) { // No corners left
00132     m_origin.setValid(false);
00133     m_axes[0].setValid(false);
00134     m_axes[1].setValid(false);
00135     return _WFMATH_POLY2REORIENT_NONE;
00136   }
00137 
00138   assert(m_origin.isValid());
00139 
00140   if(!m_axes[0].isValid())
00141     return _WFMATH_POLY2REORIENT_NONE;
00142 
00143   // Check that we still span both axes
00144 
00145   bool still_valid[2] = {false,}, got_ratio = false;
00146   CoordType ratio, size;
00147   double epsilon;
00148   int i, end = poly.numCorners();
00149 
00150   // scale epsilon
00151   for(i = 0; i < end; ++i) {
00152     if(i == skip)
00153       continue;
00154     const Point<2> &p = poly[i];
00155     CoordType x = fabs(p[0]), y = fabs(p[1]), max = (x > y) ? x : y;
00156     if(i == 0 || max < size)
00157       size = max;
00158   }
00159   int exponent;
00160   (void) frexp(size, &exponent);
00161   epsilon = ldexp(WFMATH_EPSILON, exponent);
00162 
00163   i = 0;
00164   if(skip == 0)
00165     ++i;
00166   assert(i != end);
00167   Point<2> first_point = poly[i];
00168   first_point.setValid(); // in case someone stuck invalid points in the poly
00169 
00170   while(++i != end) {
00171     if(i == skip)
00172       continue;
00173 
00174     Vector<2> diff = poly[i] - first_point;
00175     if(diff.sqrMag() < epsilon * epsilon) // No addition to span
00176       continue;
00177     if(!m_axes[1].isValid()) // We span 1D
00178       return _WFMATH_POLY2REORIENT_NONE;
00179     for(int j = 0; j < 2; ++j) {
00180       if(fabs(diff[j]) < epsilon) {
00181         assert(diff[j ? 0 : 1] >= epsilon); // because diff != 0
00182         if(still_valid[j ? 0 : 1] || got_ratio) // We span a 2D space
00183           return _WFMATH_POLY2REORIENT_NONE;
00184         still_valid[j] = true;
00185       }
00186     }
00187     // The point has both elements nonzero
00188     if(still_valid[0] || still_valid[1]) // We span a 2D space
00189       return _WFMATH_POLY2REORIENT_NONE;
00190     CoordType new_ratio = diff[1] / diff[0];
00191     if(!got_ratio) {
00192       ratio = new_ratio;
00193       got_ratio = true;
00194       continue;
00195     }
00196     if(!Equal(ratio, new_ratio)) // We span a 2D space
00197       return _WFMATH_POLY2REORIENT_NONE;
00198   }
00199 
00200   // Okay, we don't span both vectors. What now?
00201 
00202   if(still_valid[0]) {
00203     assert(m_axes[1].isValid());
00204     assert(!still_valid[1]);
00205     assert(!got_ratio);
00206     // This is easy, m_axes[1] is just invalid
00207     m_origin += m_axes[1] * first_point[1];
00208     m_axes[1].setValid(false);
00209     return _WFMATH_POLY2REORIENT_CLEAR_AXIS2;
00210   }
00211 
00212   if(still_valid[1]) {
00213     assert(m_axes[1].isValid());
00214     assert(!got_ratio);
00215     // This is a little harder, m_axes[0] is invalid, must swap axes
00216     m_origin += m_axes[0] * first_point[0];
00217     m_axes[0] = m_axes[1];
00218     m_axes[1].setValid(false);
00219     return _WFMATH_POLY2REORIENT_MOVE_AXIS2_TO_AXIS1;
00220   }
00221 
00222   // The !m_axes[1].isValid() case reducing to a point falls into here
00223   if(!got_ratio) { // Nothing's valid, all points are equal
00224     m_origin += m_axes[0] * first_point[0];
00225     if(m_axes[1].isValid())
00226       m_origin += m_axes[1] * first_point[1];
00227     m_axes[0].setValid(false);
00228     m_axes[1].setValid(false);
00229     return _WFMATH_POLY2REORIENT_CLEAR_BOTH_AXES;
00230   }
00231 
00232   assert(m_axes[1].isValid());
00233 
00234   // All the points are colinear, along some line which is not parallel
00235   // to either of the original axes
00236 
00237   Vector<dim> new0;
00238   new0 = m_axes[0] + m_axes[1] * ratio;
00239   CoordType norm = new0.mag();
00240   new0 /= norm;
00241 
00242 //  Vector diff = m_axes[0] * first_point[0] + m_axes[1] * first_point[1];
00243 //  // Causes Dot(diff, m_axes[0]) to vanish, so the point on the line
00244 //  // with x coordinate zero is the new origin
00245 //  diff -= new0 * (first_point[0] * norm);
00246 //  m_origin += diff;
00247   // or, equivalently
00248     m_origin += m_axes[1] * (first_point[1] - ratio * first_point[0]);
00249 
00250   m_axes[0] = new0;
00251   m_axes[1].setValid(false);
00252   return _Poly2Reorient(_WFMATH_POLY2REORIENT_SCALE1_CLEAR2, norm);
00253 }
00254 
00255 template<const int dim>
00256 inline void _Poly2Orient<dim>::rotate(const RotMatrix<dim>& m, const Point<dim>& p)
00257 {
00258   m_origin.rotate(m, p);
00259 
00260   for(int j = 0; j < 2; ++j)
00261     m_axes[j] = Prod(m_axes[j], m);
00262 }
00263 
00264 template<const int dim>
00265 void _Poly2Orient<dim>::rotate2(const RotMatrix<dim>& m, const Point<2>& p)
00266 {
00267   assert(m_origin.isValid());
00268 
00269   if(!m_axes[0].isValid()) {
00270     assert(p[0] == 0 && p[1] == 0);
00271     return;
00272   }
00273 
00274   Vector<dim> shift = m_axes[0] * p[0];
00275   m_axes[0] = Prod(m_axes[0], m);
00276 
00277   if(m_axes[1].isValid()) {
00278     shift += m_axes[1] * p[1];
00279     m_axes[1] = Prod(m_axes[1], m);
00280   }
00281   else
00282     assert(p[1] == 0);
00283 
00284   m_origin += shift - Prod(shift, m);
00285 }
00286 
00287 template<>
00288 inline void _Poly2Orient<3>::rotate(const Quaternion& q, const Point<3>& p)
00289 {
00290   m_origin.rotate(q, p);
00291 
00292   for(int j = 0; j < 2; ++j)
00293     m_axes[j].rotate(q);
00294 }
00295 
00296 template<>
00297 inline void _Poly2Orient<3>::rotate2(const Quaternion& q, const Point<2>& p)
00298 {
00299   assert(m_origin.isValid());
00300 
00301   if(!m_axes[0].isValid()) {
00302     assert(p[0] == 0 && p[1] == 0);
00303     return;
00304   }
00305 
00306   Vector<3> shift = m_axes[0] * p[0];
00307   m_axes[0].rotate(q);
00308 
00309   if(m_axes[1].isValid()) {
00310     shift += m_axes[1] * p[1];
00311     m_axes[1].rotate(q);
00312   }
00313   else
00314     assert(p[1] == 0);
00315 
00316   m_origin += shift - shift.rotate(q);
00317 }
00318 
00319 template<const int dim>
00320 inline bool Polygon<dim>::addCorner(int i, const Point<dim>& p, double epsilon)
00321 {
00322   Point<2> p2;
00323   bool succ = m_orient.expand(p, p2, epsilon);
00324   if(succ)
00325     m_poly.addCorner(i, p2, epsilon);
00326   return succ;
00327 }
00328 
00329 template<const int dim>
00330 inline void Polygon<dim>::removeCorner(int i)
00331 {
00332   m_poly.removeCorner(i);
00333   _Poly2Reorient r = m_orient.reduce(m_poly);
00334   r.reorient(m_poly);
00335 }
00336 
00337 template<const int dim>
00338 inline bool Polygon<dim>::moveCorner(int i, const Point<dim>& p, double epsilon)
00339 {
00340   _Poly2Orient<dim> try_orient = m_orient;
00341   _Poly2Reorient r = try_orient.reduce(m_poly, i);
00342   Point<2> p2;
00343 
00344   if(!try_orient.expand(p, p2, epsilon))
00345     return false;
00346 
00347   r.reorient(m_poly, i);
00348   m_poly[i] = p2;
00349   m_orient = try_orient;
00350 
00351   return true;
00352 }
00353 
00354 template<const int dim>
00355 AxisBox<dim> Polygon<dim>::boundingBox() const
00356 {
00357   assert(m_poly.numCorners() > 0);
00358 
00359   Point<dim> min = m_orient.convert(m_poly[0]), max = min;
00360   bool valid = min.isValid();
00361 
00362   for(int i = 1; i != m_poly.numCorners(); ++i) {
00363     Point<dim> p = m_orient.convert(m_poly[i]);
00364     valid = valid && p.isValid();
00365     for(int j = 0; j < dim; ++j) {
00366       if(p[j] < min[j])
00367         min[j] = p[j];
00368       if(p[j] > max[j])
00369         max[j] = p[j];
00370     }
00371   }
00372 
00373   min.setValid(valid);
00374   max.setValid(valid);
00375 
00376   return AxisBox<dim>(min, max, true);
00377 }
00378 
00379 template<const int dim>
00380 inline Ball<dim> Polygon<dim>::boundingSphere() const
00381 {
00382   Ball<2> b = m_poly.boundingSphere();
00383 
00384   return Ball<dim>(m_orient.convert(b.center()), b.radius());
00385 }
00386 
00387 template<const int dim>
00388 inline Ball<dim> Polygon<dim>::boundingSphereSloppy() const
00389 {
00390   Ball<2> b = m_poly.boundingSphereSloppy();
00391 
00392   return Ball<dim>(m_orient.convert(b.center()), b.radius());
00393 }
00394 
00395 } // namespace WFMath
00396 
00397 #endif  // WFMATH_POLYGON_FUNCS_H