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/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   // The same polygon can be expressed in different ways in the interal
00053   // format, so we have to call getCorner();
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; // initialize
00089   p2.setValid();
00090 
00091   if(!m_origin.isValid()) { // Adding to an empty list
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) // shift is effectively zero
00105       return true;
00106 
00107     if(j == 2) { // Have two axes, shift doesn't lie in their plane
00108       p2.setValid(false);
00109       return false;
00110     }
00111 
00112     if(!m_axes[j].isValid()) { // Didn't span this previously, now we do
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]; // shift is now perpendicular to m_axes[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)) { // No corners left
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   // Check that we still span both axes
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   // scale epsilon
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(); // in case someone stuck invalid points in the poly
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) // No addition to span
00173       continue;
00174     if(!m_axes[1].isValid()) // We span 1D
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); // because diff != 0
00179         if(still_valid[j ? 0 : 1] || got_ratio) // We span a 2D space
00180           return _WFMATH_POLY2REORIENT_NONE;
00181         still_valid[j] = true;
00182       }
00183     }
00184     // The point has both elements nonzero
00185     if(still_valid[0] || still_valid[1]) // We span a 2D space
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)) // We span a 2D space
00194       return _WFMATH_POLY2REORIENT_NONE;
00195   }
00196 
00197   // Okay, we don't span both vectors. What now?
00198 
00199   if(still_valid[0]) {
00200     assert(m_axes[1].isValid());
00201     assert(!still_valid[1]);
00202     assert(!got_ratio);
00203     // This is easy, m_axes[1] is just invalid
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     // This is a little harder, m_axes[0] is invalid, must swap axes
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   // The !m_axes[1].isValid() case reducing to a point falls into here
00220   if(!got_ratio) { // Nothing's valid, all points are equal
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   // All the points are colinear, along some line which is not parallel
00232   // to either of the original axes
00233 
00234   Vector<dim> new0;
00235   new0 = m_axes[0] + m_axes[1] * ratio;
00236   CoordType norm = new0.mag();
00237   new0 /= norm;
00238 
00239 //  Vector diff = m_axes[0] * first_point[0] + m_axes[1] * first_point[1];
00240 //  // Causes Dot(diff, m_axes[0]) to vanish, so the point on the line
00241 //  // with x coordinate zero is the new origin
00242 //  diff -= new0 * (first_point[0] * norm);
00243 //  m_origin += diff;
00244   // or, equivalently
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 } // namespace WFMath
00393 
00394 #endif  // WFMATH_POLYGON_FUNCS_H

Generated on Mon Jul 23 16:08:46 2007 for WFMath by  doxygen 1.5.2