Main Page | Namespace List | Class List | File List | Namespace Members | Class 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 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 Tue Jul 27 21:41:56 2004 for WFMath by doxygen 1.3.7