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

Generated on Wed May 28 09:20:33 2003 for WFMath by doxygen1.3-rc3