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_MATH_MATRIX3_HH_
19 #define IGNITION_MATH_MATRIX3_HH_
20 
21 #include <algorithm>
22 #include <cstring>
23 #include <ignition/math/Vector3.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
30  template <typename T> class Quaternion;
31 
34  template<typename T>
35  class Matrix3
36  {
38  public: static const Matrix3<T> Identity;
39 
41  public: static const Matrix3<T> Zero;
42 
44  public: Matrix3()
45  {
46  std::memset(this->data, 0, sizeof(this->data[0][0])*9);
47  }
48 
51  public: Matrix3(const Matrix3<T> &_m)
52  {
53  std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
54  }
55 
66  public: Matrix3(T _v00, T _v01, T _v02,
67  T _v10, T _v11, T _v12,
68  T _v20, T _v21, T _v22)
69  {
70  this->data[0][0] = _v00;
71  this->data[0][1] = _v01;
72  this->data[0][2] = _v02;
73  this->data[1][0] = _v10;
74  this->data[1][1] = _v11;
75  this->data[1][2] = _v12;
76  this->data[2][0] = _v20;
77  this->data[2][1] = _v21;
78  this->data[2][2] = _v22;
79  }
80 
83  public: Matrix3(const Quaternion<T> &_q)
84  {
85  Quaternion<T> qt = _q;
86  qt.Normalize();
87  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
88  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
89  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
90  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
91  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
92  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
93  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
94  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
95  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
96  }
97 
99  public: virtual ~Matrix3() {}
100 
111  public: void Set(T _v00, T _v01, T _v02,
112  T _v10, T _v11, T _v12,
113  T _v20, T _v21, T _v22)
114  {
115  this->data[0][0] = _v00;
116  this->data[0][1] = _v01;
117  this->data[0][2] = _v02;
118  this->data[1][0] = _v10;
119  this->data[1][1] = _v11;
120  this->data[1][2] = _v12;
121  this->data[2][0] = _v20;
122  this->data[2][1] = _v21;
123  this->data[2][2] = _v22;
124  }
125 
130  public: void Axes(const Vector3<T> &_xAxis,
131  const Vector3<T> &_yAxis,
132  const Vector3<T> &_zAxis)
133  {
134  this->Col(0, _xAxis);
135  this->Col(1, _yAxis);
136  this->Col(2, _zAxis);
137  }
138 
142  public: void Axis(const Vector3<T> &_axis, T _angle)
143  {
144  T c = cos(_angle);
145  T s = sin(_angle);
146  T C = 1-c;
147 
148  this->data[0][0] = _axis.X()*_axis.X()*C + c;
149  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
150  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
151 
152  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
153  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
154  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
155 
156  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
157  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
158  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
159  }
160 
167  public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
168  {
169  const T _v1LengthSquared = _v1.SquaredLength();
170  if (_v1LengthSquared <= 0.0)
171  {
172  // zero vector - we can't handle this
173  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
174  return;
175  }
176 
177  const T _v2LengthSquared = _v2.SquaredLength();
178  if (_v2LengthSquared <= 0.0)
179  {
180  // zero vector - we can't handle this
181  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
182  return;
183  }
184 
185  const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
186  if (fabs(dot - 1.0) <= 1e-6)
187  {
188  // the vectors are parallel
189  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
190  return;
191  }
192  else if (fabs(dot + 1.0) <= 1e-6)
193  {
194  // the vectors are opposite
195  this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
196  return;
197  }
198 
199  const Vector3<T> cross = _v1.Cross(_v2).Normalize();
200 
201  this->Axis(cross, acos(dot));
202  }
203 
207  public: void Col(unsigned int _c, const Vector3<T> &_v)
208  {
209  if (_c >= 3)
210  throw IndexException();
211 
212  this->data[0][_c] = _v.X();
213  this->data[1][_c] = _v.Y();
214  this->data[2][_c] = _v.Z();
215  }
216 
218  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
219  {
220  return Matrix3<T>(
221  this->data[0][0] - _m(0, 0),
222  this->data[0][1] - _m(0, 1),
223  this->data[0][2] - _m(0, 2),
224  this->data[1][0] - _m(1, 0),
225  this->data[1][1] - _m(1, 1),
226  this->data[1][2] - _m(1, 2),
227  this->data[2][0] - _m(2, 0),
228  this->data[2][1] - _m(2, 1),
229  this->data[2][2] - _m(2, 2));
230  }
231 
233  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
234  {
235  return Matrix3<T>(
236  this->data[0][0]+_m(0, 0),
237  this->data[0][1]+_m(0, 1),
238  this->data[0][2]+_m(0, 2),
239  this->data[1][0]+_m(1, 0),
240  this->data[1][1]+_m(1, 1),
241  this->data[1][2]+_m(1, 2),
242  this->data[2][0]+_m(2, 0),
243  this->data[2][1]+_m(2, 1),
244  this->data[2][2]+_m(2, 2));
245  }
246 
248  public: Matrix3<T> operator*(const T &_s) const
249  {
250  return Matrix3<T>(
251  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
252  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
253  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
254  }
255 
259  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
260  {
261  return Matrix3<T>(
262  // first row
263  this->data[0][0]*_m(0, 0)+
264  this->data[0][1]*_m(1, 0)+
265  this->data[0][2]*_m(2, 0),
266 
267  this->data[0][0]*_m(0, 1)+
268  this->data[0][1]*_m(1, 1)+
269  this->data[0][2]*_m(2, 1),
270 
271  this->data[0][0]*_m(0, 2)+
272  this->data[0][1]*_m(1, 2)+
273  this->data[0][2]*_m(2, 2),
274 
275  // second row
276  this->data[1][0]*_m(0, 0)+
277  this->data[1][1]*_m(1, 0)+
278  this->data[1][2]*_m(2, 0),
279 
280  this->data[1][0]*_m(0, 1)+
281  this->data[1][1]*_m(1, 1)+
282  this->data[1][2]*_m(2, 1),
283 
284  this->data[1][0]*_m(0, 2)+
285  this->data[1][1]*_m(1, 2)+
286  this->data[1][2]*_m(2, 2),
287 
288  // third row
289  this->data[2][0]*_m(0, 0)+
290  this->data[2][1]*_m(1, 0)+
291  this->data[2][2]*_m(2, 0),
292 
293  this->data[2][0]*_m(0, 1)+
294  this->data[2][1]*_m(1, 1)+
295  this->data[2][2]*_m(2, 1),
296 
297  this->data[2][0]*_m(0, 2)+
298  this->data[2][1]*_m(1, 2)+
299  this->data[2][2]*_m(2, 2));
300  }
301 
306  public: Vector3<T> operator*(const Vector3<T> &_vec) const
307  {
308  return Vector3<T>(
309  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
310  this->data[0][2]*_vec.Z(),
311  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
312  this->data[1][2]*_vec.Z(),
313  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
314  this->data[2][2]*_vec.Z());
315  }
316 
321  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
322  {
323  return _m * _s;
324  }
325 
332  public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
333  const Matrix3<T> &_m)
334  {
335  return Vector3<T>(
336  _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
337  _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
338  _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
339  }
340 
346  public: bool Equal(const Matrix3 &_m, const T &_tol) const
347  {
348  return equal<T>(this->data[0][0], _m(0, 0), _tol)
349  && equal<T>(this->data[0][1], _m(0, 1), _tol)
350  && equal<T>(this->data[0][2], _m(0, 2), _tol)
351  && equal<T>(this->data[1][0], _m(1, 0), _tol)
352  && equal<T>(this->data[1][1], _m(1, 1), _tol)
353  && equal<T>(this->data[1][2], _m(1, 2), _tol)
354  && equal<T>(this->data[2][0], _m(2, 0), _tol)
355  && equal<T>(this->data[2][1], _m(2, 1), _tol)
356  && equal<T>(this->data[2][2], _m(2, 2), _tol);
357  }
358 
362  public: bool operator==(const Matrix3<T> &_m) const
363  {
364  return this->Equal(_m, static_cast<T>(1e-6));
365  }
366 
370  public: bool operator!=(const Matrix3<T> &_m) const
371  {
372  return !(*this == _m);
373  }
374 
378  public: inline const T &operator()(size_t _row, size_t _col) const
379  {
380  if (_row >= 3 || _col >= 3)
381  throw IndexException();
382  return this->data[_row][_col];
383  }
384 
388  public: inline T &operator()(size_t _row, size_t _col)
389  {
390  if (_row >= 3 || _col >=3)
391  throw IndexException();
392  return this->data[_row][_col];
393  }
394 
397  public: T Determinant() const
398  {
399  T t0 = this->data[2][2]*this->data[1][1]
400  - this->data[2][1]*this->data[1][2];
401 
402  T t1 = -(this->data[2][2]*this->data[1][0]
403  -this->data[2][0]*this->data[1][2]);
404 
405  T t2 = this->data[2][1]*this->data[1][0]
406  - this->data[2][0]*this->data[1][1];
407 
408  return t0 * this->data[0][0]
409  + t1 * this->data[0][1]
410  + t2 * this->data[0][2];
411  }
412 
415  public: Matrix3<T> Inverse() const
416  {
417  T t0 = this->data[2][2]*this->data[1][1] -
418  this->data[2][1]*this->data[1][2];
419 
420  T t1 = -(this->data[2][2]*this->data[1][0] -
421  this->data[2][0]*this->data[1][2]);
422 
423  T t2 = this->data[2][1]*this->data[1][0] -
424  this->data[2][0]*this->data[1][1];
425 
426  T invDet = 1.0 / (t0 * this->data[0][0] +
427  t1 * this->data[0][1] +
428  t2 * this->data[0][2]);
429 
430  return invDet * Matrix3<T>(
431  t0,
432  - (this->data[2][2] * this->data[0][1] -
433  this->data[2][1] * this->data[0][2]),
434  + (this->data[1][2] * this->data[0][1] -
435  this->data[1][1] * this->data[0][2]),
436  t1,
437  + (this->data[2][2] * this->data[0][0] -
438  this->data[2][0] * this->data[0][2]),
439  - (this->data[1][2] * this->data[0][0] -
440  this->data[1][0] * this->data[0][2]),
441  t2,
442  - (this->data[2][1] * this->data[0][0] -
443  this->data[2][0] * this->data[0][1]),
444  + (this->data[1][1] * this->data[0][0] -
445  this->data[1][0] * this->data[0][1]));
446  }
447 
449  public: void Transpose()
450  {
451  std::swap(this->data[0][1], this->data[1][0]);
452  std::swap(this->data[0][2], this->data[2][0]);
453  std::swap(this->data[1][2], this->data[2][1]);
454  }
455 
458  public: Matrix3<T> Transposed() const
459  {
460  return Matrix3<T>(
461  this->data[0][0], this->data[1][0], this->data[2][0],
462  this->data[0][1], this->data[1][1], this->data[2][1],
463  this->data[0][2], this->data[1][2], this->data[2][2]);
464  }
465 
470  public: friend std::ostream &operator<<(
471  std::ostream &_out, const ignition::math::Matrix3<T> &_m)
472  {
473  _out << precision(_m(0, 0), 6) << " "
474  << precision(_m(0, 1), 6) << " "
475  << precision(_m(0, 2), 6) << " "
476  << precision(_m(1, 0), 6) << " "
477  << precision(_m(1, 1), 6) << " "
478  << precision(_m(1, 2), 6) << " "
479  << precision(_m(2, 0), 6) << " "
480  << precision(_m(2, 1), 6) << " "
481  << precision(_m(2, 2), 6);
482 
483  return _out;
484  }
489  public: friend std::istream &operator>>(
490  std::istream &_in, ignition::math::Matrix3<T> &_m)
491  {
492  // Skip white spaces
493  _in.setf(std::ios_base::skipws);
494  T d[9];
495  _in >> d[0] >> d[1] >> d[2]
496  >> d[3] >> d[4] >> d[5]
497  >> d[6] >> d[7] >> d[8];
498 
499  _m.Set(d[0], d[1], d[2],
500  d[3], d[4], d[5],
501  d[6], d[7], d[8]);
502  return _in;
503  }
504 
506  private: T data[3][3];
507  };
508 
509  template<typename T>
511  1, 0, 0,
512  0, 1, 0,
513  0, 0, 1);
514 
515  template<typename T>
517  0, 0, 0,
518  0, 0, 0,
519  0, 0, 0);
520 
524  }
525 }
526 
527 #endif
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition: Matrix3.hh:378
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition: Matrix3.hh:370
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix3< T > &_m)
Stream extraction operator.
Definition: Matrix3.hh:489
Matrix3< double > Matrix3d
Definition: Matrix3.hh:522
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:363
Matrix3< int > Matrix3i
Definition: Matrix3.hh:521
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition: Matrix3.hh:51
Matrix3()
Constructor.
Definition: Matrix3.hh:44
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix3.hh:397
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition: Matrix3.hh:388
friend Vector3< T > operator*(const Vector3< T > &_v, const Matrix3< T > &_m)
Matrix left multiplication operator for Vector3.
Definition: Matrix3.hh:332
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator with Vector3 on the right treated like a column vector.
Definition: Matrix3.hh:306
void From2Axes(const Vector3< T > &_v1, const Vector3< T > &_v2)
Set the matrix to represent rotation from vector _v1 to vector _v2, so that _v2.Normalize() == this *...
Definition: Matrix3.hh:167
const T & Y() const
Get the y component.
Definition: Quaternion.hh:926
Matrix3< T > operator*(const T &_s) const
returns the element wise scalar multiplication
Definition: Matrix3.hh:248
T X() const
Get the x value.
Definition: Vector3.hh:639
const T & Z() const
Get the z component.
Definition: Quaternion.hh:933
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:196
static const Matrix3< T > Zero
Zero matrix.
Definition: Matrix3.hh:41
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:186
friend Matrix3< T > operator*(T _s, const Matrix3< T > &_m)
Matrix multiplication operator for scaling.
Definition: Matrix3.hh:321
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition: Matrix3.hh:415
virtual ~Matrix3()
Desctructor.
Definition: Matrix3.hh:99
T Y() const
Get the y value.
Definition: Vector3.hh:646
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition: Matrix3.hh:207
A 3x3 matrix class.
Definition: Matrix3.hh:35
Matrix3< float > Matrix3f
Definition: Matrix3.hh:523
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition: Matrix3.hh:362
T Z() const
Get the z value.
Definition: Vector3.hh:653
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:259
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition: Matrix3.hh:233
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:458
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
static const Matrix3< T > Identity
Identity matrix.
Definition: Matrix3.hh:38
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:111
Matrix3< T > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition: Matrix3.hh:218
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:121
void Transpose()
Transpose this matrix.
Definition: Matrix3.hh:449
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix3< T > &_m)
Stream insertion operator.
Definition: Matrix3.hh:470
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:130
const T & W() const
Get the w component.
Definition: Quaternion.hh:912
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition: Matrix3.hh:83
Definition: AffineException.hh:30
void Axis(const Vector3< T > &_axis, T _angle)
Set the matrix from an axis and angle.
Definition: Matrix3.hh:142
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:220
A quaternion class.
Definition: Matrix3.hh:30
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition: Matrix3.hh:66
bool Equal(const Matrix3 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix3.hh:346
const T & X() const
Get the x component.
Definition: Quaternion.hh:919