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