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