WFMath  1.0.2
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 <limits>
00038 
00039 #include <cmath>
00040 
00041 #include <cassert>
00042 
00043 namespace WFMath {
00044 
00045 template<int dim>
00046 Vector<dim>::Vector(const Vector<dim>& v) : m_valid(v.m_valid)
00047 {
00048   for(int i = 0; i < dim; ++i) {
00049     m_elem[i] = v.m_elem[i];
00050   }
00051 }
00052 
00053 template<int dim>
00054 Vector<dim>::Vector(const Point<dim>& p) : m_valid(p.isValid())
00055 {
00056   for(int i = 0; i < dim; ++i) {
00057     m_elem[i] = p.elements()[i];
00058   }
00059 }
00060 
00061 template<int dim>
00062 const Vector<dim>& Vector<dim>::ZERO()
00063 {
00064   static ZeroPrimitive<Vector<dim> > zeroVector(dim);
00065   return zeroVector.getShape();
00066 }
00067 
00068 
00069 template<int dim>
00070 Vector<dim>& Vector<dim>::operator=(const Vector<dim>& v)
00071 {
00072   m_valid = v.m_valid;
00073 
00074   for(int i = 0; i < dim; ++i) {
00075     m_elem[i] = v.m_elem[i];
00076   }
00077 
00078   return *this;
00079 }
00080 
00081 template<int dim>
00082 bool Vector<dim>::isEqualTo(const Vector<dim>& v, CoordType epsilon) const
00083 {
00084   double delta = _ScaleEpsilon(m_elem, v.m_elem, dim, epsilon);
00085 
00086   for(int i = 0; i < dim; ++i) {
00087     if(std::fabs(m_elem[i] - v.m_elem[i]) > delta) {
00088       return false;
00089     }
00090   }
00091 
00092   return true;
00093 }
00094 
00095 template <int dim>
00096 Vector<dim>& operator+=(Vector<dim>& v1, const Vector<dim>& v2)
00097 {
00098   v1.m_valid = v1.m_valid && v2.m_valid;
00099 
00100   for(int i = 0; i < dim; ++i) {
00101     v1.m_elem[i] += v2.m_elem[i];
00102   }
00103 
00104   return v1;
00105 }
00106 
00107 template <int dim>
00108 Vector<dim>& operator-=(Vector<dim>& v1, const Vector<dim>& v2)
00109 {
00110   v1.m_valid = v1.m_valid && v2.m_valid;
00111 
00112   for(int i = 0; i < dim; ++i) {
00113     v1.m_elem[i] -= v2.m_elem[i];
00114   }
00115 
00116   return v1;
00117 }
00118 
00119 template <int dim>
00120 Vector<dim>& operator*=(Vector<dim>& v, CoordType d)
00121 {
00122   for(int i = 0; i < dim; ++i) {
00123     v.m_elem[i] *= d;
00124   }
00125 
00126   return v;
00127 }
00128 
00129 template <int dim>
00130 Vector<dim>& operator/=(Vector<dim>& v, CoordType d)
00131 {
00132   for(int i = 0; i < dim; ++i) {
00133     v.m_elem[i] /= d;
00134   }
00135 
00136   return v;
00137 }
00138 
00139 
00140 template <int dim>
00141 Vector<dim> operator-(const Vector<dim>& v)
00142 {
00143   Vector<dim> ans;
00144 
00145   ans.m_valid = v.m_valid;
00146 
00147   for(int i = 0; i < dim; ++i) {
00148     ans.m_elem[i] = -v.m_elem[i];
00149   }
00150 
00151   return ans;
00152 }
00153 
00154 template<int dim>
00155 Vector<dim>& Vector<dim>::sloppyNorm(CoordType norm)
00156 {
00157   CoordType mag = sloppyMag();
00158 
00159   assert("need nonzero length vector" && mag > norm / std::numeric_limits<CoordType>::max());
00160 
00161   return (*this *= norm / mag);
00162 }
00163 
00164 template<int dim>
00165 Vector<dim>& Vector<dim>::zero()
00166 {
00167   m_valid = true;
00168 
00169   for(int i = 0; i < dim; ++i) {
00170     m_elem[i] = 0;
00171   }
00172 
00173   return *this;
00174 }
00175 
00176 template<int dim>
00177 CoordType Angle(const Vector<dim>& v, const Vector<dim>& u)
00178 {
00179   // Adding numbers with large magnitude differences can cause
00180   // a loss of precision, but Dot() checks for this now
00181 
00182   CoordType dp = FloatClamp(Dot(u, v) / std::sqrt(u.sqrMag() * v.sqrMag()),
00183                          -1.0, 1.0);
00184 
00185   CoordType angle = std::acos(dp);
00186  
00187   return angle;
00188 }
00189 
00190 template<int dim>
00191 Vector<dim>& Vector<dim>::rotate(int axis1, int axis2, CoordType theta)
00192 {
00193   assert(axis1 >= 0 && axis2 >= 0 && axis1 < dim && axis2 < dim && axis1 != axis2);
00194 
00195   CoordType tmp1 = m_elem[axis1], tmp2 = m_elem[axis2];
00196   CoordType stheta = std::sin(theta),
00197             ctheta = std::cos(theta);
00198 
00199   m_elem[axis1] = tmp1 * ctheta - tmp2 * stheta;
00200   m_elem[axis2] = tmp2 * ctheta + tmp1 * stheta;
00201 
00202   return *this;
00203 }
00204 
00205 template<int dim>
00206 Vector<dim>& Vector<dim>::rotate(const Vector<dim>& v1, const Vector<dim>& v2,
00207                                  CoordType theta)
00208 {
00209   RotMatrix<dim> m;
00210   return operator=(Prod(*this, m.rotation(v1, v2, theta)));
00211 }
00212 
00213 template<int dim>
00214 Vector<dim>& Vector<dim>::rotate(const RotMatrix<dim>& m)
00215 {
00216   return *this = Prod(*this, m);
00217 }
00218 
00219 template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta);
00220 template<> Vector<3>& Vector<3>::rotate(const Quaternion& q);
00221 
00222 template<int dim>
00223 CoordType Dot(const Vector<dim>& v1, const Vector<dim>& v2)
00224 {
00225   double delta = _ScaleEpsilon(v1.m_elem, v2.m_elem, dim);
00226 
00227   CoordType ans = 0;
00228 
00229   for(int i = 0; i < dim; ++i) {
00230     ans += v1.m_elem[i] * v2.m_elem[i];
00231   }
00232 
00233   return (std::fabs(ans) >= delta) ? ans : 0;
00234 }
00235 
00236 template<int dim>
00237 CoordType Vector<dim>::sqrMag() const
00238 {
00239   CoordType ans = 0;
00240 
00241   for(int i = 0; i < dim; ++i) {
00242     // all terms > 0, no loss of precision through cancelation
00243     ans += m_elem[i] * m_elem[i];
00244   }
00245 
00246   return ans;
00247 }
00248 
00249 template<int dim>
00250 bool Perpendicular(const Vector<dim>& v1, const Vector<dim>& v2)
00251 {
00252   CoordType max1 = 0, max2 = 0;
00253 
00254   for(int i = 0; i < dim; ++i) {
00255     CoordType val1 = std::fabs(v1[i]), val2 = std::fabs(v2[i]);
00256     if(val1 > max1) {
00257       max1 = val1;
00258     }
00259     if(val2 > max2) {
00260       max2 = val2;
00261     }
00262   }
00263 
00264   // Need to scale by both, since Dot(v1, v2) goes like the product of the magnitudes
00265   int exp1, exp2;
00266   (void) std::frexp(max1, &exp1);
00267   (void) std::frexp(max2, &exp2);
00268 
00269   return std::fabs(Dot(v1, v2)) < std::ldexp(numeric_constants<CoordType>::epsilon(), exp1 + exp2);
00270 }
00271 
00272 // Note for people trying to compute the above numbers
00273 // more accurately:
00274 
00275 // The worst value for dim == 2 occurs when the ratio of the components
00276 // of the vector is sqrt(2) - 1. The value above is equal to sqrt(4 - 2 * sqrt(2)).
00277 
00278 // The worst value for dim == 3 occurs when the two smaller components
00279 // are equal, and their ratio with the large component is the
00280 // (unique, real) solution to the equation q x^3 + (q-1) x + p == 0,
00281 // where p = sqrt(2) - 1, and q = sqrt(3) + 1 - 2 * sqrt(2).
00282 // Running the script bc_sloppy_mag_3 provided with the WFMath source
00283 // will calculate the above number.
00284 
00285 template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta);
00286 template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const;
00287 
00288 template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta,
00289                                        CoordType z);
00290 template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta,
00291                                    CoordType& z) const;
00292 template<> Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta,
00293                                            CoordType phi);
00294 template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
00295                                        CoordType& phi) const;
00296 
00297 template<> CoordType Vector<2>::sloppyMag() const;
00298 template<> CoordType Vector<3>::sloppyMag() const;
00299 
00300 template<> CoordType Vector<1>::sloppyMag() const
00301         {return std::fabs(m_elem[0]);}
00302 
00303 template<> Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true)
00304         {m_elem[0] = x; m_elem[1] = y;}
00305 template<> Vector<3>::Vector(CoordType x, CoordType y, CoordType z) : m_valid(true)
00306         {m_elem[0] = x; m_elem[1] = y; m_elem[2] = z;}
00307 
00308 template<> Vector<2>& Vector<2>::rotate(CoordType theta)
00309         {return rotate(0, 1, theta);}
00310 
00311 template<> Vector<3>& Vector<3>::rotateX(CoordType theta)
00312         {return rotate(1, 2, theta);}
00313 template<> Vector<3>& Vector<3>::rotateY(CoordType theta)
00314         {return rotate(2, 0, theta);}
00315 template<> Vector<3>& Vector<3>::rotateZ(CoordType theta)
00316         {return rotate(0, 1, theta);}
00317 
00318 
00319 } // namespace WFMath
00320 
00321 #endif // WFMATH_VECTOR_FUNCS_H