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

Generated for WFMath by  doxygen 1.5.4