WFMath 0.3.11

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 
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