WFMath
1.0.1
|
00001 // rotmatrix.h (RotMatrix<> class definition) 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 #ifndef WFMATH_ROTMATRIX_H 00027 #define WFMATH_ROTMATRIX_H 00028 00029 #include <wfmath/const.h> 00030 00031 #include <iosfwd> 00032 00033 namespace WFMath { 00034 00036 template<int dim> // m1 * m2 00037 RotMatrix<dim> Prod(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2); 00039 template<int dim> // m1 * m2^-1 00040 RotMatrix<dim> ProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2); 00042 template<int dim> // m1^-1 * m2 00043 RotMatrix<dim> InvProd(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2); 00045 template<int dim> // m1^-1 * m2^-1 00046 RotMatrix<dim> InvProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2); 00047 00048 template<int dim> // m * v 00049 Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v); 00050 template<int dim> // m^-1 * v 00051 Vector<dim> InvProd(const RotMatrix<dim>& m, const Vector<dim>& v); 00052 template<int dim> // v * m 00053 Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m); 00054 template<int dim> // v * m^-1 00055 Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m); 00056 00058 template<int dim> 00059 RotMatrix<dim> operator*(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2); 00060 template<int dim> 00061 Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v); 00062 template<int dim> 00063 Vector<dim> operator*(const Vector<dim>& v, const RotMatrix<dim>& m); 00064 00065 template<int dim> 00066 std::ostream& operator<<(std::ostream& os, const RotMatrix<dim>& m); 00067 template<int dim> 00068 std::istream& operator>>(std::istream& is, RotMatrix<dim>& m); 00069 00071 00086 template<int dim = 3> 00087 class RotMatrix { 00088 public: 00090 RotMatrix() : m_flip(false), m_valid(false), m_age(0) {} 00092 RotMatrix(const RotMatrix& m); 00093 00094 friend std::ostream& operator<< <dim>(std::ostream& os, const RotMatrix& m); 00095 friend std::istream& operator>> <dim>(std::istream& is, RotMatrix& m); 00096 00097 RotMatrix& operator=(const RotMatrix& m); 00098 // No operator=(CoordType d[dim][dim]), since it can fail. 00099 // Use setVals() instead. 00100 00101 bool isEqualTo(const RotMatrix& m, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const; 00102 00103 bool operator==(const RotMatrix& m) const {return isEqualTo(m);} 00104 bool operator!=(const RotMatrix& m) const {return !isEqualTo(m);} 00105 00106 bool isValid() const {return m_valid;} 00107 00109 RotMatrix& identity(); 00110 00112 CoordType elem(const int i, const int j) const {return m_elem[i][j];} 00113 00115 00122 bool setVals(const CoordType vals[dim][dim], CoordType precision = numeric_constants<CoordType>::epsilon()); 00124 00131 bool setVals(const CoordType vals[dim*dim], CoordType precision = numeric_constants<CoordType>::epsilon()); 00132 00134 Vector<dim> row(const int i) const; 00136 Vector<dim> column(const int i) const; 00137 00139 CoordType trace() const; 00141 00144 CoordType determinant() const {return (m_flip ? -1.f : 1.f);} 00146 00149 RotMatrix inverse() const; 00151 00154 bool parity() const {return m_flip;} 00155 00156 // documented outside the class 00157 00158 friend RotMatrix Prod<dim> (const RotMatrix& m1, const RotMatrix& m2); 00159 friend RotMatrix ProdInv<dim> (const RotMatrix& m1, const RotMatrix& m2); 00160 friend RotMatrix InvProd<dim> (const RotMatrix& m1, const RotMatrix& m2); 00161 friend RotMatrix InvProdInv<dim> (const RotMatrix& m1, const RotMatrix& m2); 00162 friend Vector<dim> Prod<dim> (const RotMatrix& m, const Vector<dim>& v); 00163 friend Vector<dim> InvProd<dim> (const RotMatrix& m, const Vector<dim>& v); 00164 00165 // Set the value to a given rotation 00166 00168 RotMatrix& rotation (const int i, const int j, CoordType theta); 00170 00173 RotMatrix& rotation (const Vector<dim>& v1, const Vector<dim>& v2, 00174 CoordType theta); 00176 00181 RotMatrix& rotation (const Vector<dim>& from, const Vector<dim>& to); 00182 00183 // Set the value to mirror image about a certain axis 00184 00186 RotMatrix& mirror(const int i); 00188 RotMatrix& mirror(const Vector<dim>& v); 00190 00193 RotMatrix& mirror(); 00194 00196 RotMatrix& rotate(const RotMatrix& m) {return *this = Prod(*this, m);} 00197 00199 void normalize(); 00201 unsigned age() const {return m_age;} 00202 00203 // 2D/3D stuff 00204 00206 00212 RotMatrix(const Quaternion& q, const bool not_flip = true); 00213 00215 RotMatrix& rotation(CoordType theta) 00216 {return rotation(0, 1, theta);} 00217 00219 RotMatrix& rotationX(CoordType theta) {return rotation(1, 2, theta);} 00221 RotMatrix& rotationY(CoordType theta) {return rotation(2, 0, theta);} 00223 RotMatrix& rotationZ(CoordType theta) {return rotation(0, 1, theta);} 00225 RotMatrix& rotation(const Vector<dim>& axis, CoordType theta); 00227 00230 RotMatrix& rotation(const Vector<dim>& axis); // angle taken from magnitude of axis 00231 00233 00239 RotMatrix& fromQuaternion(const Quaternion& q, const bool not_flip = true); 00240 00242 RotMatrix& rotate(const Quaternion&); 00243 00245 RotMatrix& mirrorX() {return mirror(0);} 00247 RotMatrix& mirrorY() {return mirror(1);} 00249 RotMatrix& mirrorZ(); 00250 00251 private: 00252 CoordType m_elem[dim][dim]; 00253 bool m_flip; // True if the matrix is parity odd 00254 bool m_valid; 00255 unsigned m_age; 00256 00257 // Backend to setVals() above, also used in fromStream() 00258 bool _setVals(CoordType *vals, CoordType precision = numeric_constants<CoordType>::epsilon()); 00259 void checkNormalization() {if(m_age >= WFMATH_MAX_NORM_AGE && m_valid) normalize();} 00260 }; 00261 00262 template<> 00263 inline RotMatrix<3>& RotMatrix<3>::mirrorZ() 00264 { 00265 return mirror(2); 00266 } 00267 00268 template<int dim> 00269 inline RotMatrix<dim>& RotMatrix<dim>::mirror(const int i) 00270 { 00271 identity(); 00272 m_elem[i][i] = -1; 00273 m_flip = true; 00274 // m_valid and m_age already set correctly 00275 00276 return *this; 00277 } 00278 00279 } // namespace WFMath 00280 00281 #endif // WFMATH_ROTMATRIX_H