Main Page | Namespace List | Class List | File List | Namespace Members | Class Members

vector_funcs.h

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

Generated on Thu Jul 29 07:09:56 2004 for WFMath by doxygen 1.3.7