WFMath: rotmatrix.h Source File

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