WFMath  1.0.1
rotmatrix.h
1 // rotmatrix.h (RotMatrix<> class definition)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2001 The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 
23 // Author: Ron Steinke
24 // Created: 2001-12-7
25 
26 #ifndef WFMATH_ROTMATRIX_H
27 #define WFMATH_ROTMATRIX_H
28 
29 #include <wfmath/const.h>
30 
31 #include <iosfwd>
32 
33 namespace WFMath {
34 
36 template<int dim> // m1 * m2
37 RotMatrix<dim> Prod(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2);
39 template<int dim> // m1 * m2^-1
40 RotMatrix<dim> ProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2);
42 template<int dim> // m1^-1 * m2
43 RotMatrix<dim> InvProd(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2);
45 template<int dim> // m1^-1 * m2^-1
46 RotMatrix<dim> InvProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2);
47 
48 template<int dim> // m * v
49 Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v);
50 template<int dim> // m^-1 * v
51 Vector<dim> InvProd(const RotMatrix<dim>& m, const Vector<dim>& v);
52 template<int dim> // v * m
53 Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m);
54 template<int dim> // v * m^-1
55 Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m);
56 
58 template<int dim>
59 RotMatrix<dim> operator*(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2);
60 template<int dim>
61 Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v);
62 template<int dim>
63 Vector<dim> operator*(const Vector<dim>& v, const RotMatrix<dim>& m);
64 
65 template<int dim>
66 std::ostream& operator<<(std::ostream& os, const RotMatrix<dim>& m);
67 template<int dim>
68 std::istream& operator>>(std::istream& is, RotMatrix<dim>& m);
69 
71 
86 template<int dim = 3>
87 class RotMatrix {
88  public:
90  RotMatrix() : m_flip(false), m_valid(false), m_age(0) {}
92  RotMatrix(const RotMatrix& m);
93 
94  friend std::ostream& operator<< <dim>(std::ostream& os, const RotMatrix& m);
95  friend std::istream& operator>> <dim>(std::istream& is, RotMatrix& m);
96 
97  RotMatrix& operator=(const RotMatrix& m);
98  // No operator=(CoordType d[dim][dim]), since it can fail.
99  // Use setVals() instead.
100 
101  bool isEqualTo(const RotMatrix& m, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;
102 
103  bool operator==(const RotMatrix& m) const {return isEqualTo(m);}
104  bool operator!=(const RotMatrix& m) const {return !isEqualTo(m);}
105 
106  bool isValid() const {return m_valid;}
107 
109  RotMatrix& identity();
110 
112  CoordType elem(const int i, const int j) const {return m_elem[i][j];}
113 
115 
122  bool setVals(const CoordType vals[dim][dim], CoordType precision = numeric_constants<CoordType>::epsilon());
124 
131  bool setVals(const CoordType vals[dim*dim], CoordType precision = numeric_constants<CoordType>::epsilon());
132 
134  Vector<dim> row(const int i) const;
136  Vector<dim> column(const int i) const;
137 
139  CoordType trace() const;
141 
144  CoordType determinant() const {return (m_flip ? -1.f : 1.f);}
146 
149  RotMatrix inverse() const;
151 
154  bool parity() const {return m_flip;}
155 
156  // documented outside the class
157 
158  friend RotMatrix Prod<dim> (const RotMatrix& m1, const RotMatrix& m2);
159  friend RotMatrix ProdInv<dim> (const RotMatrix& m1, const RotMatrix& m2);
160  friend RotMatrix InvProd<dim> (const RotMatrix& m1, const RotMatrix& m2);
161  friend RotMatrix InvProdInv<dim> (const RotMatrix& m1, const RotMatrix& m2);
162  friend Vector<dim> Prod<dim> (const RotMatrix& m, const Vector<dim>& v);
163  friend Vector<dim> InvProd<dim> (const RotMatrix& m, const Vector<dim>& v);
164 
165  // Set the value to a given rotation
166 
168  RotMatrix& rotation (const int i, const int j, CoordType theta);
170 
173  RotMatrix& rotation (const Vector<dim>& v1, const Vector<dim>& v2,
174  CoordType theta);
176 
181  RotMatrix& rotation (const Vector<dim>& from, const Vector<dim>& to);
182 
183  // Set the value to mirror image about a certain axis
184 
186  RotMatrix& mirror(const int i);
188  RotMatrix& mirror(const Vector<dim>& v);
190 
193  RotMatrix& mirror();
194 
196  RotMatrix& rotate(const RotMatrix& m) {return *this = Prod(*this, m);}
197 
199  void normalize();
201  unsigned age() const {return m_age;}
202 
203  // 2D/3D stuff
204 
206 
212  RotMatrix(const Quaternion& q, const bool not_flip = true);
213 
216  {return rotation(0, 1, theta);}
217 
219  RotMatrix& rotationX(CoordType theta) {return rotation(1, 2, theta);}
221  RotMatrix& rotationY(CoordType theta) {return rotation(2, 0, theta);}
223  RotMatrix& rotationZ(CoordType theta) {return rotation(0, 1, theta);}
225  RotMatrix& rotation(const Vector<dim>& axis, CoordType theta);
227 
230  RotMatrix& rotation(const Vector<dim>& axis); // angle taken from magnitude of axis
231 
233 
239  RotMatrix& fromQuaternion(const Quaternion& q, const bool not_flip = true);
240 
242  RotMatrix& rotate(const Quaternion&);
243 
245  RotMatrix& mirrorX() {return mirror(0);}
247  RotMatrix& mirrorY() {return mirror(1);}
249  RotMatrix& mirrorZ();
250 
251  private:
252  CoordType m_elem[dim][dim];
253  bool m_flip; // True if the matrix is parity odd
254  bool m_valid;
255  unsigned m_age;
256 
257  // Backend to setVals() above, also used in fromStream()
258  bool _setVals(CoordType *vals, CoordType precision = numeric_constants<CoordType>::epsilon());
259  void checkNormalization() {if(m_age >= WFMATH_MAX_NORM_AGE && m_valid) normalize();}
260 };
261 
262 template<>
263 inline RotMatrix<3>& RotMatrix<3>::mirrorZ()
264 {
265  return mirror(2);
266 }
267 
268 template<int dim>
270 {
271  identity();
272  m_elem[i][i] = -1;
273  m_flip = true;
274  // m_valid and m_age already set correctly
275 
276  return *this;
277 }
278 
279 } // namespace WFMath
280 
281 #endif // WFMATH_ROTMATRIX_H