Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

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 _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   // 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 // WARNING! This operator is for sorting only. It does not
00067 // reflect any property of the box.
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; // initialize
00108   p2.setValid();
00109 
00110   if(!m_origin.isValid()) { // Adding to an empty list
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) // shift is effectively zero
00124       return true;
00125 
00126     if(j == 2) { // Have two axes, shift doesn't lie in their plane
00127       p2.setValid(false);
00128       return false;
00129     }
00130 
00131     if(!m_axes[j].isValid()) { // Didn't span this previously, now we do
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]; // shift is now perpendicular to m_axes[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)) { // No corners left
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   // Check that we still span both axes
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   // scale epsilon
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(); // in case someone stuck invalid points in the poly
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) // No addition to span
00192       continue;
00193     if(!m_axes[1].isValid()) // We span 1D
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); // because diff != 0
00198         if(still_valid[j ? 0 : 1] || got_ratio) // We span a 2D space
00199           return _WFMATH_POLY2REORIENT_NONE;
00200         still_valid[j] = true;
00201       }
00202     }
00203     // The point has both elements nonzero
00204     if(still_valid[0] || still_valid[1]) // We span a 2D space
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)) // We span a 2D space
00213       return _WFMATH_POLY2REORIENT_NONE;
00214   }
00215 
00216   // Okay, we don't span both vectors. What now?
00217 
00218   if(still_valid[0]) {
00219     assert(m_axes[1].isValid());
00220     assert(!still_valid[1]);
00221     assert(!got_ratio);
00222     // This is easy, m_axes[1] is just invalid
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     // This is a little harder, m_axes[0] is invalid, must swap axes
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   // The !m_axes[1].isValid() case reducing to a point falls into here
00239   if(!got_ratio) { // Nothing's valid, all points are equal
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   // All the points are colinear, along some line which is not parallel
00251   // to either of the original axes
00252 
00253   Vector<dim> new0;
00254   new0 = m_axes[0] + m_axes[1] * ratio;
00255   CoordType norm = new0.mag();
00256   new0 /= norm;
00257 
00258 //  Vector diff = m_axes[0] * first_point[0] + m_axes[1] * first_point[1];
00259 //  // Causes Dot(diff, m_axes[0]) to vanish, so the point on the line
00260 //  // with x coordinate zero is the new origin
00261 //  diff -= new0 * (first_point[0] * norm);
00262 //  m_origin += diff;
00263   // or, equivalently
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 } // namespace WFMath
00380 
00381 #endif  // WFMATH_POLYGON_FUNCS_H

Generated on Wed May 28 09:20:32 2003 for WFMath by doxygen1.3-rc3