WFMath
0.3.12
|
00001 // vector_funcs.h (Vector<> template functions) 00002 // 00003 // The WorldForge Project 00004 // Copyright (C) 2001 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 // Author: Ron Steinke 00024 // Created: 2001-12-7 00025 00026 // Extensive amounts of this material come from the Vector2D 00027 // and Vector3D classes from stage/math, written by Bryce W. 00028 // Harrington, Kosh, and Jari Sundell (Rakshasa). 00029 00030 #ifndef WFMATH_VECTOR_FUNCS_H 00031 #define WFMATH_VECTOR_FUNCS_H 00032 00033 #include <wfmath/vector.h> 00034 #include <wfmath/rotmatrix.h> 00035 #include <wfmath/zero.h> 00036 00037 #include <cmath> 00038 00039 #include <cassert> 00040 00041 namespace WFMath { 00042 00043 template<int dim> 00044 Vector<dim>::Vector(const Vector<dim>& v) : m_valid(v.m_valid) 00045 { 00046 for(int i = 0; i < dim; ++i) { 00047 m_elem[i] = v.m_elem[i]; 00048 } 00049 } 00050 00051 template<int dim> 00052 Vector<dim>::Vector(const Point<dim>& p) : m_valid(p.isValid()) 00053 { 00054 for(int i = 0; i < dim; ++i) { 00055 m_elem[i] = p.elements()[i]; 00056 } 00057 } 00058 00059 template<int dim> 00060 const Vector<dim>& Vector<dim>::ZERO() 00061 { 00062 static ZeroPrimitive<Vector<dim> > zeroVector(dim); 00063 return zeroVector.getShape(); 00064 } 00065 00066 00067 template<int dim> 00068 Vector<dim>& Vector<dim>::operator=(const Vector<dim>& v) 00069 { 00070 m_valid = v.m_valid; 00071 00072 for(int i = 0; i < dim; ++i) { 00073 m_elem[i] = v.m_elem[i]; 00074 } 00075 00076 return *this; 00077 } 00078 00079 template<int dim> 00080 bool Vector<dim>::isEqualTo(const Vector<dim>& v, double epsilon) const 00081 { 00082 double delta = _ScaleEpsilon(m_elem, v.m_elem, dim, epsilon); 00083 00084 for(int i = 0; i < dim; ++i) { 00085 if(fabs(m_elem[i] - v.m_elem[i]) > delta) { 00086 return false; 00087 } 00088 } 00089 00090 return true; 00091 } 00092 00093 template <int dim> 00094 Vector<dim>& operator+=(Vector<dim>& v1, const Vector<dim>& v2) 00095 { 00096 v1.m_valid = v1.m_valid && v2.m_valid; 00097 00098 for(int i = 0; i < dim; ++i) { 00099 v1.m_elem[i] += v2.m_elem[i]; 00100 } 00101 00102 return v1; 00103 } 00104 00105 template <int dim> 00106 Vector<dim>& operator-=(Vector<dim>& v1, const Vector<dim>& v2) 00107 { 00108 v1.m_valid = v1.m_valid && v2.m_valid; 00109 00110 for(int i = 0; i < dim; ++i) { 00111 v1.m_elem[i] -= v2.m_elem[i]; 00112 } 00113 00114 return v1; 00115 } 00116 00117 template <int dim> 00118 Vector<dim>& operator*=(Vector<dim>& v, CoordType d) 00119 { 00120 for(int i = 0; i < dim; ++i) { 00121 v.m_elem[i] *= d; 00122 } 00123 00124 return v; 00125 } 00126 00127 template <int dim> 00128 Vector<dim>& operator/=(Vector<dim>& v, CoordType d) 00129 { 00130 for(int i = 0; i < dim; ++i) { 00131 v.m_elem[i] /= d; 00132 } 00133 00134 return v; 00135 } 00136 00137 template <int dim> 00138 Vector<dim> operator+(const Vector<dim>& v1, const Vector<dim>& v2) 00139 { 00140 Vector<dim> ans(v1); 00141 00142 ans += v2; 00143 00144 return ans; 00145 } 00146 00147 template <int dim> 00148 Vector<dim> operator-(const Vector<dim>& v1, const Vector<dim>& v2) 00149 { 00150 Vector<dim> ans(v1); 00151 00152 ans -= v2; 00153 00154 return ans; 00155 } 00156 00157 template <int dim> 00158 Vector<dim> operator*(const Vector<dim>& v, CoordType d) 00159 { 00160 Vector<dim> ans(v); 00161 00162 ans *= d; 00163 00164 return ans; 00165 } 00166 00167 template<int dim> 00168 Vector<dim> operator*(CoordType d, const Vector<dim>& v) 00169 { 00170 Vector<dim> ans(v); 00171 00172 ans *= d; 00173 00174 return ans; 00175 } 00176 00177 template <int dim> 00178 Vector<dim> operator/(const Vector<dim>& v, CoordType d) 00179 { 00180 Vector<dim> ans(v); 00181 00182 ans /= d; 00183 00184 return ans; 00185 } 00186 00187 template <int dim> 00188 Vector<dim> operator-(const Vector<dim>& v) 00189 { 00190 Vector<dim> ans; 00191 00192 ans.m_valid = v.m_valid; 00193 00194 for(int i = 0; i < dim; ++i) { 00195 ans.m_elem[i] = -v.m_elem[i]; 00196 } 00197 00198 return ans; 00199 } 00200 00201 template<int dim> 00202 Vector<dim>& Vector<dim>::sloppyNorm(CoordType norm) 00203 { 00204 CoordType mag = sloppyMag(); 00205 00206 assert("need nonzero length vector" && mag > norm / WFMATH_MAX); 00207 00208 return (*this *= norm / mag); 00209 } 00210 00211 template<int dim> 00212 Vector<dim>& Vector<dim>::zero() 00213 { 00214 m_valid = true; 00215 00216 for(int i = 0; i < dim; ++i) { 00217 m_elem[i] = 0; 00218 } 00219 00220 return *this; 00221 } 00222 00223 template<int dim> 00224 CoordType Angle(const Vector<dim>& v, const Vector<dim>& u) 00225 { 00226 // Adding numbers with large magnitude differences can cause 00227 // a loss of precision, but Dot() checks for this now 00228 00229 CoordType dp = FloatClamp(Dot(u, v) / std::sqrt(u.sqrMag() * v.sqrMag()), 00230 -1.0, 1.0); 00231 00232 CoordType angle = std::acos(dp); 00233 00234 return angle; 00235 } 00236 00237 template<int dim> 00238 Vector<dim>& Vector<dim>::rotate(int axis1, int axis2, CoordType theta) 00239 { 00240 assert(axis1 >= 0 && axis2 >= 0 && axis1 < dim && axis2 < dim && axis1 != axis2); 00241 00242 CoordType tmp1 = m_elem[axis1], tmp2 = m_elem[axis2]; 00243 CoordType stheta = std::sin(theta), 00244 ctheta = std::cos(theta); 00245 00246 m_elem[axis1] = tmp1 * ctheta - tmp2 * stheta; 00247 m_elem[axis2] = tmp2 * ctheta + tmp1 * stheta; 00248 00249 return *this; 00250 } 00251 00252 template<int dim> 00253 Vector<dim>& Vector<dim>::rotate(const Vector<dim>& v1, const Vector<dim>& v2, 00254 CoordType theta) 00255 { 00256 RotMatrix<dim> m; 00257 return operator=(Prod(*this, m.rotation(v1, v2, theta))); 00258 } 00259 00260 template<int dim> 00261 Vector<dim>& Vector<dim>::rotate(const RotMatrix<dim>& m) 00262 { 00263 return *this = Prod(*this, m); 00264 } 00265 00266 template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta); 00267 template<> Vector<3>& Vector<3>::rotate(const Quaternion& q); 00268 00269 template<int dim> 00270 CoordType Dot(const Vector<dim>& v1, const Vector<dim>& v2) 00271 { 00272 double delta = _ScaleEpsilon(v1.m_elem, v2.m_elem, dim); 00273 00274 CoordType ans = 0; 00275 00276 for(int i = 0; i < dim; ++i) { 00277 ans += v1.m_elem[i] * v2.m_elem[i]; 00278 } 00279 00280 return (fabs(ans) >= delta) ? ans : 0; 00281 } 00282 00283 template<int dim> 00284 CoordType Vector<dim>::sqrMag() const 00285 { 00286 CoordType ans = 0; 00287 00288 for(int i = 0; i < dim; ++i) { 00289 // all terms > 0, no loss of precision through cancelation 00290 ans += m_elem[i] * m_elem[i]; 00291 } 00292 00293 return ans; 00294 } 00295 00296 template<int dim> 00297 bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2, bool& same_dir) 00298 { 00299 CoordType dot = Dot(v1, v2); 00300 00301 same_dir = (dot > 0); 00302 00303 return Equal(dot * dot, v1.sqrMag() * v2.sqrMag()); 00304 } 00305 00306 template<int dim> 00307 bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2) 00308 { 00309 bool same_dir; 00310 00311 return Parallel(v1, v2, same_dir); 00312 } 00313 00314 template<int dim> 00315 bool Perpendicular(const Vector<dim>& v1, const Vector<dim>& v2) 00316 { 00317 double max1 = 0, max2 = 0; 00318 00319 for(int i = 0; i < dim; ++i) { 00320 double val1 = fabs(v1[i]), val2 = fabs(v2[i]); 00321 if(val1 > max1) { 00322 max1 = val1; 00323 } 00324 if(val2 > max2) { 00325 max2 = val2; 00326 } 00327 } 00328 00329 // Need to scale by both, since Dot(v1, v2) goes like the product of the magnitudes 00330 int exp1, exp2; 00331 (void) frexp(max1, &exp1); 00332 (void) frexp(max2, &exp2); 00333 00334 return fabs(Dot(v1, v2)) < ldexp(WFMATH_EPSILON, exp1 + exp2); 00335 } 00336 00337 template<> 00338 const CoordType Vector<1>::sloppyMagMax() 00339 { 00340 return (CoordType) 1; 00341 } 00342 00343 template<> 00344 const CoordType Vector<2>::sloppyMagMax() 00345 { 00346 return (CoordType) 1.082392200292393968799446410733; 00347 } 00348 00349 template<> 00350 const CoordType Vector<3>::sloppyMagMax() 00351 { 00352 return (CoordType) 1.145934719303161490541433900265; 00353 } 00354 00355 template<> 00356 const CoordType Vector<1>::sloppyMagMaxSqrt() 00357 { 00358 return (CoordType) 1; 00359 } 00360 00361 template<> 00362 const CoordType Vector<2>::sloppyMagMaxSqrt() 00363 { 00364 return (CoordType) 1.040380795811030899095785063701; 00365 } 00366 00367 template<> 00368 const CoordType Vector<3>::sloppyMagMaxSqrt() 00369 { 00370 return (CoordType) 1.070483404496847625250328653179; 00371 } 00372 00373 // Note for people trying to compute the above numbers 00374 // more accurately: 00375 00376 // The worst value for dim == 2 occurs when the ratio of the components 00377 // of the vector is sqrt(2) - 1. The value above is equal to sqrt(4 - 2 * sqrt(2)). 00378 00379 // The worst value for dim == 3 occurs when the two smaller components 00380 // are equal, and their ratio with the large component is the 00381 // (unique, real) solution to the equation q x^3 + (q-1) x + p == 0, 00382 // where p = sqrt(2) - 1, and q = sqrt(3) + 1 - 2 * sqrt(2). 00383 // Running the script bc_sloppy_mag_3 provided with the WFMath source 00384 // will calculate the above number. 00385 00386 template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta); 00387 template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const; 00388 00389 template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta, 00390 CoordType z); 00391 template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta, 00392 CoordType& z) const; 00393 template<> Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta, 00394 CoordType phi); 00395 template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta, 00396 CoordType& phi) const; 00397 00398 template<> CoordType Vector<2>::sloppyMag() const; 00399 template<> CoordType Vector<3>::sloppyMag() const; 00400 00401 template<> CoordType Vector<1>::sloppyMag() const 00402 {return std::fabs(m_elem[0]);} 00403 00404 template<> Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true) 00405 {m_elem[0] = x; m_elem[1] = y;} 00406 template<> Vector<3>::Vector(CoordType x, CoordType y, CoordType z) : m_valid(true) 00407 {m_elem[0] = x; m_elem[1] = y; m_elem[2] = z;} 00408 00409 template<> Vector<2>& Vector<2>::rotate(CoordType theta) 00410 {return rotate(0, 1, theta);} 00411 00412 template<> Vector<3>& Vector<3>::rotateX(CoordType theta) 00413 {return rotate(1, 2, theta);} 00414 template<> Vector<3>& Vector<3>::rotateY(CoordType theta) 00415 {return rotate(2, 0, theta);} 00416 template<> Vector<3>& Vector<3>::rotateZ(CoordType theta) 00417 {return rotate(0, 1, theta);} 00418 00419 00420 } // namespace WFMath 00421 00422 #endif // WFMATH_VECTOR_FUNCS_H