Matrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef _IGNITION_MATRIX3_HH_
19 #define _IGNITION_MATRIX3_HH_
20 
21 #include <algorithm>
22 #include <cstring>
23 #include <ignition/math/Vector3.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
32  template<typename T>
33  class Matrix3
34  {
36  public: static const Matrix3<T> Identity;
37 
39  public: static const Matrix3<T> Zero;
40 
42  public: Matrix3()
43  {
44  std::memset(this->data, 0, sizeof(this->data[0][0])*9);
45  }
46 
49  public: Matrix3(const Matrix3<T> &_m)
50  {
51  std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
52  }
53 
64  public: Matrix3(T _v00, T _v01, T _v02,
65  T _v10, T _v11, T _v12,
66  T _v20, T _v21, T _v22)
67  {
68  this->data[0][0] = _v00;
69  this->data[0][1] = _v01;
70  this->data[0][2] = _v02;
71  this->data[1][0] = _v10;
72  this->data[1][1] = _v11;
73  this->data[1][2] = _v12;
74  this->data[2][0] = _v20;
75  this->data[2][1] = _v21;
76  this->data[2][2] = _v22;
77  }
78 
81  public: Matrix3(const Quaternion<T> &_q)
82  {
83  Quaternion<T> qt = _q;
84  qt.Normalize();
85  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
86  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
87  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
88  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
89  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
90  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
91  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
92  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
93  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
94  }
95 
97  public: virtual ~Matrix3() {}
98 
109  public: void Set(T _v00, T _v01, T _v02,
110  T _v10, T _v11, T _v12,
111  T _v20, T _v21, T _v22)
112  {
113  this->data[0][0] = _v00;
114  this->data[0][1] = _v01;
115  this->data[0][2] = _v02;
116  this->data[1][0] = _v10;
117  this->data[1][1] = _v11;
118  this->data[1][2] = _v12;
119  this->data[2][0] = _v20;
120  this->data[2][1] = _v21;
121  this->data[2][2] = _v22;
122  }
123 
128  public: void Axes(const Vector3<T> &_xAxis,
129  const Vector3<T> &_yAxis,
130  const Vector3<T> &_zAxis)
131  {
132  this->Col(0, _xAxis);
133  this->Col(1, _yAxis);
134  this->Col(2, _zAxis);
135  }
136 
140  public: void Axis(const Vector3<T> &_axis, T _angle)
141  {
142  T c = cos(_angle);
143  T s = sin(_angle);
144  T C = 1-c;
145 
146  this->data[0][0] = _axis.X()*_axis.X()*C + c;
147  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
148  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
149 
150  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
151  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
152  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
153 
154  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
155  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
156  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
157  }
158 
162  public: void Col(unsigned int _c, const Vector3<T> &_v)
163  {
164  if (_c >= 3)
165  throw IndexException();
166 
167  this->data[0][_c] = _v.X();
168  this->data[1][_c] = _v.Y();
169  this->data[2][_c] = _v.Z();
170  }
171 
173  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
174  {
175  return Matrix3<T>(
176  this->data[0][0] - _m(0, 0),
177  this->data[0][1] - _m(0, 1),
178  this->data[0][2] - _m(0, 2),
179  this->data[1][0] - _m(1, 0),
180  this->data[1][1] - _m(1, 1),
181  this->data[1][2] - _m(1, 2),
182  this->data[2][0] - _m(2, 0),
183  this->data[2][1] - _m(2, 1),
184  this->data[2][2] - _m(2, 2));
185  }
186 
188  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
189  {
190  return Matrix3<T>(
191  this->data[0][0]+_m(0, 0),
192  this->data[0][1]+_m(0, 1),
193  this->data[0][2]+_m(0, 2),
194  this->data[1][0]+_m(1, 0),
195  this->data[1][1]+_m(1, 1),
196  this->data[1][2]+_m(1, 2),
197  this->data[2][0]+_m(2, 0),
198  this->data[2][1]+_m(2, 1),
199  this->data[2][2]+_m(2, 2));
200  }
201 
203  public: Matrix3<T> operator*(const T &_s) const
204  {
205  return Matrix3<T>(
206  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
207  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
208  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
209  }
210 
214  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
215  {
216  return Matrix3<T>(
217  // first row
218  this->data[0][0]*_m(0, 0)+
219  this->data[0][1]*_m(1, 0)+
220  this->data[0][2]*_m(2, 0),
221 
222  this->data[0][0]*_m(0, 1)+
223  this->data[0][1]*_m(1, 1)+
224  this->data[0][2]*_m(2, 1),
225 
226  this->data[0][0]*_m(0, 2)+
227  this->data[0][1]*_m(1, 2)+
228  this->data[0][2]*_m(2, 2),
229 
230  // second row
231  this->data[1][0]*_m(0, 0)+
232  this->data[1][1]*_m(1, 0)+
233  this->data[1][2]*_m(2, 0),
234 
235  this->data[1][0]*_m(0, 1)+
236  this->data[1][1]*_m(1, 1)+
237  this->data[1][2]*_m(2, 1),
238 
239  this->data[1][0]*_m(0, 2)+
240  this->data[1][1]*_m(1, 2)+
241  this->data[1][2]*_m(2, 2),
242 
243  // third row
244  this->data[2][0]*_m(0, 0)+
245  this->data[2][1]*_m(1, 0)+
246  this->data[2][2]*_m(2, 0),
247 
248  this->data[2][0]*_m(0, 1)+
249  this->data[2][1]*_m(1, 1)+
250  this->data[2][2]*_m(2, 1),
251 
252  this->data[2][0]*_m(0, 2)+
253  this->data[2][1]*_m(1, 2)+
254  this->data[2][2]*_m(2, 2));
255  }
256 
261  public: Vector3<T> operator*(const Vector3<T> &_vec) const
262  {
263  return Vector3<T>(
264  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
265  this->data[0][2]*_vec.Z(),
266  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
267  this->data[1][2]*_vec.Z(),
268  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
269  this->data[2][2]*_vec.Z());
270  }
271 
276  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
277  {
278  return _m * _s;
279  }
280 
287  public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
288  const Matrix3<T> &_m)
289  {
290  return Vector3<T>(
291  _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
292  _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
293  _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
294  }
295 
301  public: bool Equal(const Matrix3 &_m, const T &_tol) const
302  {
303  return equal<T>(this->data[0][0], _m(0, 0), _tol)
304  && equal<T>(this->data[0][1], _m(0, 1), _tol)
305  && equal<T>(this->data[0][2], _m(0, 2), _tol)
306  && equal<T>(this->data[1][0], _m(1, 0), _tol)
307  && equal<T>(this->data[1][1], _m(1, 1), _tol)
308  && equal<T>(this->data[1][2], _m(1, 2), _tol)
309  && equal<T>(this->data[2][0], _m(2, 0), _tol)
310  && equal<T>(this->data[2][1], _m(2, 1), _tol)
311  && equal<T>(this->data[2][2], _m(2, 2), _tol);
312  }
313 
317  public: bool operator==(const Matrix3<T> &_m) const
318  {
319  return this->Equal(_m, static_cast<T>(1e-6));
320  }
321 
325  public: bool operator!=(const Matrix3<T> &_m) const
326  {
327  return !(*this == _m);
328  }
329 
333  public: inline const T &operator()(size_t _row, size_t _col) const
334  {
335  if (_row >= 3 || _col >= 3)
336  throw IndexException();
337  return this->data[_row][_col];
338  }
339 
343  public: inline T &operator()(size_t _row, size_t _col)
344  {
345  if (_row >= 3 || _col >=3)
346  throw IndexException();
347  return this->data[_row][_col];
348  }
349 
352  public: T Determinant() const
353  {
354  T t0 = this->data[2][2]*this->data[1][1]
355  - this->data[2][1]*this->data[1][2];
356 
357  T t1 = -(this->data[2][2]*this->data[1][0]
358  -this->data[2][0]*this->data[1][2]);
359 
360  T t2 = this->data[2][1]*this->data[1][0]
361  - this->data[2][0]*this->data[1][1];
362 
363  return t0 * this->data[0][0]
364  + t1 * this->data[0][1]
365  + t2 * this->data[0][2];
366  }
367 
370  public: Matrix3<T> Inverse() const
371  {
372  T t0 = this->data[2][2]*this->data[1][1] -
373  this->data[2][1]*this->data[1][2];
374 
375  T t1 = -(this->data[2][2]*this->data[1][0] -
376  this->data[2][0]*this->data[1][2]);
377 
378  T t2 = this->data[2][1]*this->data[1][0] -
379  this->data[2][0]*this->data[1][1];
380 
381  T invDet = 1.0 / (t0 * this->data[0][0] +
382  t1 * this->data[0][1] +
383  t2 * this->data[0][2]);
384 
385  return invDet * Matrix3<T>(
386  t0,
387  - (this->data[2][2] * this->data[0][1] -
388  this->data[2][1] * this->data[0][2]),
389  + (this->data[1][2] * this->data[0][1] -
390  this->data[1][1] * this->data[0][2]),
391  t1,
392  + (this->data[2][2] * this->data[0][0] -
393  this->data[2][0] * this->data[0][2]),
394  - (this->data[1][2] * this->data[0][0] -
395  this->data[1][0] * this->data[0][2]),
396  t2,
397  - (this->data[2][1] * this->data[0][0] -
398  this->data[2][0] * this->data[0][1]),
399  + (this->data[1][1] * this->data[0][0] -
400  this->data[1][0] * this->data[0][1]));
401  }
402 
404  public: void Transpose()
405  {
406  std::swap(this->data[0][1], this->data[1][0]);
407  std::swap(this->data[0][2], this->data[2][0]);
408  std::swap(this->data[1][2], this->data[2][1]);
409  }
410 
413  public: Matrix3<T> Transposed() const
414  {
415  return Matrix3<T>(
416  this->data[0][0], this->data[1][0], this->data[2][0],
417  this->data[0][1], this->data[1][1], this->data[2][1],
418  this->data[0][2], this->data[1][2], this->data[2][2]);
419  }
420 
425  public: friend std::ostream &operator<<(
426  std::ostream &_out, const ignition::math::Matrix3<T> &_m)
427  {
428  _out << precision(_m(0, 0), 6) << " "
429  << precision(_m(0, 1), 6) << " "
430  << precision(_m(0, 2), 6) << " "
431  << precision(_m(1, 0), 6) << " "
432  << precision(_m(1, 1), 6) << " "
433  << precision(_m(1, 2), 6) << " "
434  << precision(_m(2, 0), 6) << " "
435  << precision(_m(2, 1), 6) << " "
436  << precision(_m(2, 2), 6);
437 
438  return _out;
439  }
444  public: friend std::istream &operator>>(
445  std::istream &_in, ignition::math::Matrix3<T> &_m)
446  {
447  // Skip white spaces
448  _in.setf(std::ios_base::skipws);
449  T d[9];
450  _in >> d[0] >> d[1] >> d[2]
451  >> d[3] >> d[4] >> d[5]
452  >> d[6] >> d[7] >> d[8];
453 
454  _m.Set(d[0], d[1], d[2],
455  d[3], d[4], d[5],
456  d[6], d[7], d[8]);
457  return _in;
458  }
459 
461  private: T data[3][3];
462  };
463 
464  template<typename T>
466  1, 0, 0,
467  0, 1, 0,
468  0, 0, 1);
469 
470  template<typename T>
472  0, 0, 0,
473  0, 0, 0,
474  0, 0, 0);
475 
479  }
480 }
481 
482 #endif
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition: Matrix3.hh:333
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition: Matrix3.hh:325
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix3< T > &_m)
Stream extraction operator.
Definition: Matrix3.hh:444
Matrix3< double > Matrix3d
Definition: Matrix3.hh:477
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:269
Matrix3< int > Matrix3i
Definition: Matrix3.hh:476
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition: Matrix3.hh:49
Matrix3()
Constructor.
Definition: Matrix3.hh:42
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix3.hh:352
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition: Matrix3.hh:343
friend Vector3< T > operator*(const Vector3< T > &_v, const Matrix3< T > &_m)
Matrix left multiplication operator for Vector3.
Definition: Matrix3.hh:287
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator with Vector3 on the right treated like a column vector.
Definition: Matrix3.hh:261
const T & Y() const
Get the y component.
Definition: Quaternion.hh:789
Matrix3< T > operator*(const T &_s) const
returns the element wise scalar multiplication
Definition: Matrix3.hh:203
T X() const
Get the x value.
Definition: Vector3.hh:630
const T & Z() const
Get the z component.
Definition: Quaternion.hh:796
static const Matrix3< T > Zero
Zero matrix.
Definition: Matrix3.hh:39
friend Matrix3< T > operator*(T _s, const Matrix3< T > &_m)
Matrix multiplication operator for scaling.
Definition: Matrix3.hh:276
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition: Matrix3.hh:370
virtual ~Matrix3()
Desctructor.
Definition: Matrix3.hh:97
T Y() const
Get the y value.
Definition: Vector3.hh:637
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition: Matrix3.hh:162
A 3x3 matrix class.
Definition: Matrix3.hh:33
Matrix3< float > Matrix3f
Definition: Matrix3.hh:478
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition: Matrix3.hh:317
T Z() const
Get the z value.
Definition: Vector3.hh:644
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
Matrix3< T > operator*(const Matrix3< T > &_m) const
Matrix multiplication operator.
Definition: Matrix3.hh:214
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition: Matrix3.hh:188
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:413
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
static const Matrix3< T > Identity
Identity matrix.
Definition: Matrix3.hh:36
void Set(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Set values.
Definition: Matrix3.hh:109
Matrix3< T > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition: Matrix3.hh:173
void Transpose()
Transpose this matrix.
Definition: Matrix3.hh:404
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix3< T > &_m)
Stream insertion operator.
Definition: Matrix3.hh:425
void Axes(const Vector3< T > &_xAxis, const Vector3< T > &_yAxis, const Vector3< T > &_zAxis)
Set the matrix from three axis (1 per column)
Definition: Matrix3.hh:128
const T & W() const
Get the w component.
Definition: Quaternion.hh:775
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition: Matrix3.hh:81
Definition: AffineException.hh:30
void Axis(const Vector3< T > &_axis, T _angle)
Set the matrix from an axis and angle.
Definition: Matrix3.hh:140
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:206
A quaternion class.
Definition: Quaternion.hh:31
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition: Matrix3.hh:64
bool Equal(const Matrix3 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix3.hh:301
const T & X() const
Get the x component.
Definition: Quaternion.hh:782