WFMath 0.3.11
|
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