WFMath 0.3.11
|
00001 // point_funcs.h (point class copied from libCoal, subsequently modified) 00002 // 00003 // The WorldForge Project 00004 // Copyright (C) 2000, 2001, 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 00027 #ifndef WFMATH_POINT_FUNCS_H 00028 #define WFMATH_POINT_FUNCS_H 00029 00030 #include <wfmath/point.h> 00031 00032 #include <wfmath/vector.h> 00033 00034 #include <cmath> 00035 00036 namespace WFMath { 00037 00038 template<const int dim> 00039 inline Point<dim>::Point(const Point<dim>& p) : m_valid(p.m_valid) 00040 { 00041 for(int i = 0; i < dim; ++i) { 00042 m_elem[i] = p.m_elem[i]; 00043 } 00044 } 00045 00046 template<const int dim> 00047 inline Point<dim>::Point(const Vector<dim>& v) : m_valid(v.isValid()) 00048 { 00049 for(int i = 0; i < dim; ++i) { 00050 m_elem[i] = v.elements()[i]; 00051 } 00052 } 00053 00054 template<const int dim> 00055 inline Point<dim>& Point<dim>::setToOrigin() 00056 { 00057 for(int i = 0; i < dim; ++i) { 00058 m_elem[i] = 0; 00059 } 00060 00061 m_valid = true; 00062 00063 return *this; 00064 } 00065 00066 template<const int dim> 00067 inline bool Point<dim>::isEqualTo(const Point<dim> &p, double epsilon) const 00068 { 00069 CoordType delta = (CoordType) _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon); 00070 00071 for(int i = 0; i < dim; ++i) { 00072 if(fabs(m_elem[i] - p.m_elem[i]) > delta) { 00073 return false; 00074 } 00075 } 00076 00077 return true; 00078 } 00079 00080 template<const int dim> 00081 inline Vector<dim> operator-(const Point<dim>& c1, const Point<dim>& c2) 00082 { 00083 Vector<dim> out; 00084 00085 for(int i = 0; i < dim; ++i) { 00086 out.m_elem[i] = c1.m_elem[i] - c2.m_elem[i]; 00087 } 00088 00089 out.m_valid = c1.m_valid && c2.m_valid; 00090 00091 return out; 00092 } 00093 00094 template<const int dim> 00095 inline Point<dim>& operator+=(Point<dim>& p, const Vector<dim> &rhs) 00096 { 00097 for(int i = 0; i < dim; ++i) { 00098 p.m_elem[i] += rhs.m_elem[i]; 00099 } 00100 00101 p.m_valid = p.m_valid && rhs.m_valid; 00102 00103 return p; 00104 } 00105 00106 template<const int dim> 00107 inline Point<dim> operator+(const Point<dim>& c, const Vector<dim>& v) 00108 { 00109 Point<dim> out(c); 00110 00111 out += v; 00112 00113 return out; 00114 } 00115 00116 template<const int dim> 00117 inline Point<dim> operator+(const Vector<dim>& v, const Point<dim>& c) 00118 { 00119 Point<dim> out(c); 00120 00121 out += v; 00122 00123 return out; 00124 } 00125 00126 template<const int dim> 00127 inline Point<dim>& operator-=(Point<dim>& p, const Vector<dim> &rhs) 00128 { 00129 for(int i = 0; i < dim; ++i) { 00130 p.m_elem[i] -= rhs.m_elem[i]; 00131 } 00132 00133 p.m_valid = p.m_valid && rhs.m_valid; 00134 00135 return p; 00136 } 00137 00138 template<const int dim> 00139 inline Point<dim> operator-(const Point<dim>& c, const Vector<dim>& v) 00140 { 00141 Point<dim> out(c); 00142 00143 out -= v; 00144 00145 return out; 00146 } 00147 00148 template<const int dim> 00149 inline Point<dim>& Point<dim>::operator=(const Point<dim>& rhs) 00150 { 00151 // Compare pointer addresses 00152 if (this == &rhs) { 00153 return *this; 00154 } 00155 00156 for(int i = 0; i < dim; ++i) { 00157 m_elem[i] = rhs.m_elem[i]; 00158 } 00159 00160 m_valid = rhs.m_valid; 00161 00162 return *this; 00163 } 00164 00165 template<const int dim> 00166 inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2) 00167 { 00168 CoordType ans = 0; 00169 00170 for(int i = 0; i < dim; ++i) { 00171 CoordType diff = p1.m_elem[i] - p2.m_elem[i]; 00172 ans += diff * diff; 00173 } 00174 00175 return (fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0; 00176 } 00177 00178 #ifndef WFMATH_NO_TEMPLATES_AS_TEMPLATE_PARAMETERS 00179 template<const int dim, template<class, class> class container, 00180 template<class, class> class container2> 00181 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c, 00182 const container2<CoordType, std::allocator<CoordType> >& weights) 00183 { 00184 // FIXME become friend 00185 00186 typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end(); 00187 typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(), 00188 w_end = weights.end(); 00189 00190 Point<dim> out; 00191 00192 if (c_i == c_end || w_i == w_end) { 00193 return out; 00194 } 00195 00196 bool valid = c_i->isValid(); 00197 00198 CoordType tot_weight = *w_i, max_weight = fabs(*w_i); 00199 for(int j = 0; j < dim; ++j) { 00200 out[j] = (*c_i)[j] * *w_i; 00201 } 00202 00203 while(++c_i != c_end && ++w_i != w_end) { 00204 tot_weight += *w_i; 00205 CoordType val = fabs(*w_i); 00206 if(val > max_weight) 00207 max_weight = val; 00208 if(!c_i->isValid()) 00209 valid = false; 00210 for(int j = 0; j < dim; ++j) 00211 out[j] += (*c_i)[j] * *w_i; 00212 } 00213 00214 // Make sure the weights don't add up to zero 00215 if (max_weight <= 0 || fabs(tot_weight) <= max_weight * WFMATH_EPSILON) { 00216 return out; 00217 } 00218 00219 for(int j = 0; j < dim; ++j) { 00220 out[j] /= tot_weight; 00221 } 00222 00223 out.setValid(valid); 00224 00225 return out; 00226 } 00227 00228 template<const int dim, template<class, class> class container> 00229 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c) 00230 { 00231 // FIXME become friend 00232 00233 typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end(); 00234 00235 if (i == end) { 00236 return Point<dim>(); 00237 } 00238 00239 Point<dim> out = *i; 00240 int num_points = 1; 00241 00242 bool valid = i->isValid(); 00243 00244 while(++i != end) { 00245 ++num_points; 00246 if(!i->isValid()) 00247 valid = false; 00248 for(int j = 0; j < dim; ++j) 00249 out[j] += (*i)[j]; 00250 } 00251 00252 for(int j = 0; j < dim; ++j) { 00253 out[j] /= num_points; 00254 } 00255 00256 out.setValid(valid); 00257 00258 return out; 00259 } 00260 #endif 00261 00262 template<const int dim> 00263 inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist) 00264 { 00265 Point<dim> out; 00266 CoordType conj_dist = 1 - dist; 00267 00268 for(int i = 0; i < dim; ++i) { 00269 out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist; 00270 } 00271 00272 out.m_valid = p1.m_valid && p2.m_valid; 00273 00274 return out; 00275 } 00276 00277 template<> Point<2>::Point(CoordType x, CoordType y) : m_valid(true) 00278 { 00279 m_elem[0] = x; 00280 m_elem[1] = y; 00281 } 00282 00283 template<> Point<3>::Point(CoordType x, CoordType y, CoordType z) : m_valid(true) 00284 { 00285 m_elem[0] = x; 00286 m_elem[1] = y; 00287 m_elem[2] = z; 00288 } 00289 00290 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION 00291 template<> Point<2>& Point<2>::polar(CoordType r, CoordType theta); 00292 template<> void Point<2>::asPolar(CoordType& r, CoordType& theta) const; 00293 00294 template<> Point<3>& Point<3>::polar(CoordType r, CoordType theta, 00295 CoordType z); 00296 template<> void Point<3>::asPolar(CoordType& r, CoordType& theta, 00297 CoordType& z) const; 00298 template<> Point<3>& Point<3>::spherical(CoordType r, CoordType theta, 00299 CoordType phi); 00300 template<> void Point<3>::asSpherical(CoordType& r, CoordType& theta, 00301 CoordType& phi) const; 00302 #else 00303 void _NCFS_Point2_polar(CoordType *m_elem, CoordType r, CoordType theta); 00304 void _NCFS_Point2_asPolar(CoordType *m_elem, CoordType& r, CoordType& theta); 00305 00306 void _NCFS_Point3_polar(CoordType *m_elem, CoordType r, CoordType theta, 00307 CoordType z); 00308 void _NCFS_Point3_asPolar(CoordType *m_elem, CoordType& r, CoordType& theta, 00309 CoordType& z); 00310 void _NCFS_Point3_spherical(CoordType *m_elem, CoordType r, CoordType theta, 00311 CoordType phi); 00312 void _NCFS_Point3_asSpherical(CoordType *m_elem, CoordType& r, CoordType& theta, 00313 CoordType& phi); 00314 00315 template<> 00316 inline Point<2>& Point<2>::polar(CoordType r, CoordType theta) 00317 { 00318 _NCFS_Point2_polar((CoordType*) m_elem, r, theta); 00319 m_valid = true; 00320 return *this; 00321 } 00322 00323 template<> 00324 inline void Point<2>::asPolar(CoordType& r, CoordType& theta) const 00325 { 00326 _NCFS_Point2_asPolar((CoordType*) m_elem, r, theta); 00327 } 00328 00329 template<> 00330 inline Point<3>& Point<3>::polar(CoordType r, CoordType theta, CoordType z) 00331 { 00332 _NCFS_Point3_polar((CoordType*) m_elem, r, theta, z); 00333 m_valid = true; 00334 return *this; 00335 } 00336 00337 template<> 00338 inline void Point<3>::asPolar(CoordType& r, CoordType& theta, CoordType& z) const 00339 { 00340 _NCFS_Point3_asPolar((CoordType*) m_elem, r, theta, z); 00341 } 00342 00343 template<> 00344 inline Point<3>& Point<3>::spherical(CoordType r, CoordType theta, CoordType phi) 00345 { 00346 _NCFS_Point3_spherical((CoordType*) m_elem, r, theta, phi); 00347 m_valid = true; 00348 return *this; 00349 } 00350 00351 template<> 00352 inline void Point<3>::asSpherical(CoordType& r, CoordType& theta, CoordType& phi) const 00353 { 00354 _NCFS_Point3_asSpherical((CoordType*) m_elem, r, theta, phi); 00355 } 00356 #endif 00357 00358 } // namespace WFMath 00359 00360 #endif // WFMATH_POINT_FUNCS_H