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 Fri Dec 30 12:07:54 2005 for WFMath by  doxygen 1.4.5