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