WFMath
1.0.2
|
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, CoordType epsilon) const 00077 { 00078 CoordType delta = _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon); 00079 00080 for(int i = 0; i < dim; ++i) { 00081 if(std::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-=(Point<dim>& p, const Vector<dim> &rhs) 00117 { 00118 for(int i = 0; i < dim; ++i) { 00119 p.m_elem[i] -= rhs.m_elem[i]; 00120 } 00121 00122 p.m_valid = p.m_valid && rhs.m_valid; 00123 00124 return p; 00125 } 00126 00127 template<int dim> 00128 inline Point<dim>& Point<dim>::operator=(const Point<dim>& rhs) 00129 { 00130 // Compare pointer addresses 00131 if (this == &rhs) { 00132 return *this; 00133 } 00134 00135 for(int i = 0; i < dim; ++i) { 00136 m_elem[i] = rhs.m_elem[i]; 00137 } 00138 00139 m_valid = rhs.m_valid; 00140 00141 return *this; 00142 } 00143 00144 template<int dim> 00145 inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2) 00146 { 00147 CoordType ans = 0; 00148 00149 for(int i = 0; i < dim; ++i) { 00150 CoordType diff = p1.m_elem[i] - p2.m_elem[i]; 00151 ans += diff * diff; 00152 } 00153 00154 return (std::fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0; 00155 } 00156 00157 template<int dim, template<class, class> class container, 00158 template<class, class> class container2> 00159 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c, 00160 const container2<CoordType, std::allocator<CoordType> >& weights) 00161 { 00162 // FIXME become friend 00163 00164 typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end(); 00165 typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(), 00166 w_end = weights.end(); 00167 00168 Point<dim> out; 00169 00170 if (c_i == c_end || w_i == w_end) { 00171 return out; 00172 } 00173 00174 bool valid = c_i->isValid(); 00175 00176 CoordType tot_weight = *w_i, max_weight = std::fabs(*w_i); 00177 for(int j = 0; j < dim; ++j) { 00178 out[j] = (*c_i)[j] * *w_i; 00179 } 00180 00181 while(++c_i != c_end && ++w_i != w_end) { 00182 tot_weight += *w_i; 00183 CoordType val = std::fabs(*w_i); 00184 if(val > max_weight) 00185 max_weight = val; 00186 if(!c_i->isValid()) 00187 valid = false; 00188 for(int j = 0; j < dim; ++j) 00189 out[j] += (*c_i)[j] * *w_i; 00190 } 00191 00192 // Make sure the weights don't add up to zero 00193 if (max_weight <= 0 || std::fabs(tot_weight) <= max_weight * numeric_constants<CoordType>::epsilon()) { 00194 return out; 00195 } 00196 00197 for(int j = 0; j < dim; ++j) { 00198 out[j] /= tot_weight; 00199 } 00200 00201 out.setValid(valid); 00202 00203 return out; 00204 } 00205 00206 template<int dim, template<class, class> class container> 00207 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c) 00208 { 00209 // FIXME become friend 00210 00211 typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end(); 00212 00213 if (i == end) { 00214 return Point<dim>(); 00215 } 00216 00217 Point<dim> out = *i; 00218 float num_points = 1; 00219 00220 bool valid = i->isValid(); 00221 00222 while(++i != end) { 00223 ++num_points; 00224 if(!i->isValid()) 00225 valid = false; 00226 for(int j = 0; j < dim; ++j) 00227 out[j] += (*i)[j]; 00228 } 00229 00230 for(int j = 0; j < dim; ++j) { 00231 out[j] /= num_points; 00232 } 00233 00234 out.setValid(valid); 00235 00236 return out; 00237 } 00238 00239 template<int dim> 00240 inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist) 00241 { 00242 Point<dim> out; 00243 CoordType conj_dist = 1 - dist; 00244 00245 for(int i = 0; i < dim; ++i) { 00246 out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist; 00247 } 00248 00249 out.m_valid = p1.m_valid && p2.m_valid; 00250 00251 return out; 00252 } 00253 00254 template<> Point<2>& Point<2>::polar(CoordType r, CoordType theta); 00255 template<> void Point<2>::asPolar(CoordType& r, CoordType& theta) const; 00256 00257 template<> Point<3>& Point<3>::polar(CoordType r, CoordType theta, 00258 CoordType z); 00259 template<> void Point<3>::asPolar(CoordType& r, CoordType& theta, 00260 CoordType& z) const; 00261 template<> Point<3>& Point<3>::spherical(CoordType r, CoordType theta, 00262 CoordType phi); 00263 template<> void Point<3>::asSpherical(CoordType& r, CoordType& theta, 00264 CoordType& phi) const; 00265 00266 } // namespace WFMath 00267 00268 #endif // WFMATH_POINT_FUNCS_H