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