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