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