Matrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 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 #ifndef IGNITION_MATH_MATRIX3_HH_
18 #define IGNITION_MATH_MATRIX3_HH_
19 
20 #include <algorithm>
21 #include <cstring>
22 #include <ignition/math/Helpers.hh>
23 #include <ignition/math/Vector3.hh>
25 #include <ignition/math/config.hh>
26 
27 namespace ignition
28 {
29  namespace math
30  {
31  inline namespace IGNITION_MATH_VERSION_NAMESPACE
32  {
33  template <typename T> class Quaternion;
34 
37  template<typename T>
38  class Matrix3
39  {
41  public: static const Matrix3<T> Identity;
42 
44  public: static const Matrix3<T> Zero;
45 
47  public: Matrix3()
48  {
49  std::memset(this->data, 0, sizeof(this->data[0][0])*9);
50  }
51 
54  public: Matrix3(const Matrix3<T> &_m)
55  {
56  std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
57  }
58 
69  public: Matrix3(T _v00, T _v01, T _v02,
70  T _v10, T _v11, T _v12,
71  T _v20, T _v21, T _v22)
72  {
73  this->data[0][0] = _v00;
74  this->data[0][1] = _v01;
75  this->data[0][2] = _v02;
76  this->data[1][0] = _v10;
77  this->data[1][1] = _v11;
78  this->data[1][2] = _v12;
79  this->data[2][0] = _v20;
80  this->data[2][1] = _v21;
81  this->data[2][2] = _v22;
82  }
83 
86  public: explicit Matrix3(const Quaternion<T> &_q)
87  {
88  Quaternion<T> qt = _q;
89  qt.Normalize();
90  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
91  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
92  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
93  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
94  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
95  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
96  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
97  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
98  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
99  }
100 
102  public: virtual ~Matrix3() {}
103 
114  public: void Set(T _v00, T _v01, T _v02,
115  T _v10, T _v11, T _v12,
116  T _v20, T _v21, T _v22)
117  {
118  this->data[0][0] = _v00;
119  this->data[0][1] = _v01;
120  this->data[0][2] = _v02;
121  this->data[1][0] = _v10;
122  this->data[1][1] = _v11;
123  this->data[1][2] = _v12;
124  this->data[2][0] = _v20;
125  this->data[2][1] = _v21;
126  this->data[2][2] = _v22;
127  }
128 
133  public: void Axes(const Vector3<T> &_xAxis,
134  const Vector3<T> &_yAxis,
135  const Vector3<T> &_zAxis)
136  {
137  this->Col(0, _xAxis);
138  this->Col(1, _yAxis);
139  this->Col(2, _zAxis);
140  }
141 
145  public: void Axis(const Vector3<T> &_axis, T _angle)
146  {
147  T c = cos(_angle);
148  T s = sin(_angle);
149  T C = 1-c;
150 
151  this->data[0][0] = _axis.X()*_axis.X()*C + c;
152  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
153  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
154 
155  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
156  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
157  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
158 
159  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
160  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
161  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
162  }
163 
170  public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
171  {
172  const T _v1LengthSquared = _v1.SquaredLength();
173  if (_v1LengthSquared <= 0.0)
174  {
175  // zero vector - we can't handle this
176  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
177  return;
178  }
179 
180  const T _v2LengthSquared = _v2.SquaredLength();
181  if (_v2LengthSquared <= 0.0)
182  {
183  // zero vector - we can't handle this
184  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
185  return;
186  }
187 
188  const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
189  if (fabs(dot - 1.0) <= 1e-6)
190  {
191  // the vectors are parallel
192  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
193  return;
194  }
195  else if (fabs(dot + 1.0) <= 1e-6)
196  {
197  // the vectors are opposite
198  this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
199  return;
200  }
201 
202  const Vector3<T> cross = _v1.Cross(_v2).Normalize();
203 
204  this->Axis(cross, acos(dot));
205  }
206 
211  public: void Col(unsigned int _c, const Vector3<T> &_v)
212  {
213  unsigned int c = clamp(_c, 0u, 2u);
214 
215  this->data[0][c] = _v.X();
216  this->data[1][c] = _v.Y();
217  this->data[2][c] = _v.Z();
218  }
219 
223  public: Matrix3<T> &operator=(const Matrix3<T> &_mat)
224  {
225  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*9);
226  return *this;
227  }
228 
230  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
231  {
232  return Matrix3<T>(
233  this->data[0][0] - _m(0, 0),
234  this->data[0][1] - _m(0, 1),
235  this->data[0][2] - _m(0, 2),
236  this->data[1][0] - _m(1, 0),
237  this->data[1][1] - _m(1, 1),
238  this->data[1][2] - _m(1, 2),
239  this->data[2][0] - _m(2, 0),
240  this->data[2][1] - _m(2, 1),
241  this->data[2][2] - _m(2, 2));
242  }
243 
245  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
246  {
247  return Matrix3<T>(
248  this->data[0][0]+_m(0, 0),
249  this->data[0][1]+_m(0, 1),
250  this->data[0][2]+_m(0, 2),
251  this->data[1][0]+_m(1, 0),
252  this->data[1][1]+_m(1, 1),
253  this->data[1][2]+_m(1, 2),
254  this->data[2][0]+_m(2, 0),
255  this->data[2][1]+_m(2, 1),
256  this->data[2][2]+_m(2, 2));
257  }
258 
260  public: Matrix3<T> operator*(const T &_s) const
261  {
262  return Matrix3<T>(
263  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
264  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
265  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
266  }
267 
271  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
272  {
273  return Matrix3<T>(
274  // first row
275  this->data[0][0]*_m(0, 0)+
276  this->data[0][1]*_m(1, 0)+
277  this->data[0][2]*_m(2, 0),
278 
279  this->data[0][0]*_m(0, 1)+
280  this->data[0][1]*_m(1, 1)+
281  this->data[0][2]*_m(2, 1),
282 
283  this->data[0][0]*_m(0, 2)+
284  this->data[0][1]*_m(1, 2)+
285  this->data[0][2]*_m(2, 2),
286 
287  // second row
288  this->data[1][0]*_m(0, 0)+
289  this->data[1][1]*_m(1, 0)+
290  this->data[1][2]*_m(2, 0),
291 
292  this->data[1][0]*_m(0, 1)+
293  this->data[1][1]*_m(1, 1)+
294  this->data[1][2]*_m(2, 1),
295 
296  this->data[1][0]*_m(0, 2)+
297  this->data[1][1]*_m(1, 2)+
298  this->data[1][2]*_m(2, 2),
299 
300  // third row
301  this->data[2][0]*_m(0, 0)+
302  this->data[2][1]*_m(1, 0)+
303  this->data[2][2]*_m(2, 0),
304 
305  this->data[2][0]*_m(0, 1)+
306  this->data[2][1]*_m(1, 1)+
307  this->data[2][2]*_m(2, 1),
308 
309  this->data[2][0]*_m(0, 2)+
310  this->data[2][1]*_m(1, 2)+
311  this->data[2][2]*_m(2, 2));
312  }
313 
318  public: Vector3<T> operator*(const Vector3<T> &_vec) const
319  {
320  return Vector3<T>(
321  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
322  this->data[0][2]*_vec.Z(),
323  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
324  this->data[1][2]*_vec.Z(),
325  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
326  this->data[2][2]*_vec.Z());
327  }
328 
333  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
334  {
335  return _m * _s;
336  }
337 
344  public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
345  const Matrix3<T> &_m)
346  {
347  return Vector3<T>(
348  _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
349  _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
350  _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
351  }
352 
358  public: bool Equal(const Matrix3 &_m, const T &_tol) const
359  {
360  return equal<T>(this->data[0][0], _m(0, 0), _tol)
361  && equal<T>(this->data[0][1], _m(0, 1), _tol)
362  && equal<T>(this->data[0][2], _m(0, 2), _tol)
363  && equal<T>(this->data[1][0], _m(1, 0), _tol)
364  && equal<T>(this->data[1][1], _m(1, 1), _tol)
365  && equal<T>(this->data[1][2], _m(1, 2), _tol)
366  && equal<T>(this->data[2][0], _m(2, 0), _tol)
367  && equal<T>(this->data[2][1], _m(2, 1), _tol)
368  && equal<T>(this->data[2][2], _m(2, 2), _tol);
369  }
370 
374  public: bool operator==(const Matrix3<T> &_m) const
375  {
376  return this->Equal(_m, static_cast<T>(1e-6));
377  }
378 
382  public: Matrix3<T> &operator=(const Quaternion<T> &_q)
383  {
384  return *this = Matrix3<T>(_q);
385  }
386 
390  public: bool operator!=(const Matrix3<T> &_m) const
391  {
392  return !(*this == _m);
393  }
394 
399  public: inline const T &operator()(size_t _row, size_t _col) const
400  {
401  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
403  }
404 
409  public: inline T &operator()(size_t _row, size_t _col)
410  {
411  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
413  }
414 
417  public: T Determinant() const
418  {
419  T t0 = this->data[2][2]*this->data[1][1]
420  - this->data[2][1]*this->data[1][2];
421 
422  T t1 = -(this->data[2][2]*this->data[1][0]
423  -this->data[2][0]*this->data[1][2]);
424 
425  T t2 = this->data[2][1]*this->data[1][0]
426  - this->data[2][0]*this->data[1][1];
427 
428  return t0 * this->data[0][0]
429  + t1 * this->data[0][1]
430  + t2 * this->data[0][2];
431  }
432 
435  public: Matrix3<T> Inverse() const
436  {
437  T t0 = this->data[2][2]*this->data[1][1] -
438  this->data[2][1]*this->data[1][2];
439 
440  T t1 = -(this->data[2][2]*this->data[1][0] -
441  this->data[2][0]*this->data[1][2]);
442 
443  T t2 = this->data[2][1]*this->data[1][0] -
444  this->data[2][0]*this->data[1][1];
445 
446  T invDet = 1.0 / (t0 * this->data[0][0] +
447  t1 * this->data[0][1] +
448  t2 * this->data[0][2]);
449 
450  return invDet * Matrix3<T>(
451  t0,
452  - (this->data[2][2] * this->data[0][1] -
453  this->data[2][1] * this->data[0][2]),
454  + (this->data[1][2] * this->data[0][1] -
455  this->data[1][1] * this->data[0][2]),
456  t1,
457  + (this->data[2][2] * this->data[0][0] -
458  this->data[2][0] * this->data[0][2]),
459  - (this->data[1][2] * this->data[0][0] -
460  this->data[1][0] * this->data[0][2]),
461  t2,
462  - (this->data[2][1] * this->data[0][0] -
463  this->data[2][0] * this->data[0][1]),
464  + (this->data[1][1] * this->data[0][0] -
465  this->data[1][0] * this->data[0][1]));
466  }
467 
469  public: void Transpose()
470  {
471  std::swap(this->data[0][1], this->data[1][0]);
472  std::swap(this->data[0][2], this->data[2][0]);
473  std::swap(this->data[1][2], this->data[2][1]);
474  }
475 
478  public: Matrix3<T> Transposed() const
479  {
480  return Matrix3<T>(
481  this->data[0][0], this->data[1][0], this->data[2][0],
482  this->data[0][1], this->data[1][1], this->data[2][1],
483  this->data[0][2], this->data[1][2], this->data[2][2]);
484  }
485 
490  public: friend std::ostream &operator<<(
491  std::ostream &_out, const ignition::math::Matrix3<T> &_m)
492  {
493  _out << precision(_m(0, 0), 6) << " "
494  << precision(_m(0, 1), 6) << " "
495  << precision(_m(0, 2), 6) << " "
496  << precision(_m(1, 0), 6) << " "
497  << precision(_m(1, 1), 6) << " "
498  << precision(_m(1, 2), 6) << " "
499  << precision(_m(2, 0), 6) << " "
500  << precision(_m(2, 1), 6) << " "
501  << precision(_m(2, 2), 6);
502 
503  return _out;
504  }
509  public: friend std::istream &operator>>(
510  std::istream &_in, ignition::math::Matrix3<T> &_m)
511  {
512  // Skip white spaces
513  _in.setf(std::ios_base::skipws);
514  T d[9];
515  _in >> d[0] >> d[1] >> d[2]
516  >> d[3] >> d[4] >> d[5]
517  >> d[6] >> d[7] >> d[8];
518 
519  _m.Set(d[0], d[1], d[2],
520  d[3], d[4], d[5],
521  d[6], d[7], d[8]);
522  return _in;
523  }
524 
526  private: T data[3][3];
527  };
528 
529  template<typename T>
530  const Matrix3<T> Matrix3<T>::Identity(
531  1, 0, 0,
532  0, 1, 0,
533  0, 0, 1);
534 
535  template<typename T>
536  const Matrix3<T> Matrix3<T>::Zero(
537  0, 0, 0,
538  0, 0, 0,
539  0, 0, 0);
540 
544  }
545  }
546 }
547 
548 #endif
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition: Matrix3.hh:86
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:579
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:133
Matrix3< T > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition: Matrix3.hh:230
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition: Matrix3.hh:211
void Axis(const Vector3< T > &_axis, T _angle)
Set the matrix from an axis and angle.
Definition: Matrix3.hh:145
Matrix3< float > Matrix3f
Definition: Matrix3.hh:543
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:170
A quaternion class.
Definition: Matrix3.hh:33
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition: Matrix3.hh:374
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition: Matrix3.hh:390
T Y() const
Get the y value.
Definition: Vector3.hh:654
bool Equal(const Matrix3 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix3.hh:358
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:188
const T & Z() const
Get the z component.
Definition: Quaternion.hh:969
T Z() const
Get the z value.
Definition: Vector3.hh:661
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:216
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition: Matrix3.hh:69
const T & W() const
Get the w component.
Definition: Quaternion.hh:948
const T & Y() const
Get the y component.
Definition: Quaternion.hh:962
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition: Matrix3.hh:245
virtual ~Matrix3()
Desctructor.
Definition: Matrix3.hh:102
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition: Matrix3.hh:399
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:123
Matrix3< double > Matrix3d
Definition: Matrix3.hh:542
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition: Matrix3.hh:54
const T & X() const
Get the x component.
Definition: Quaternion.hh:955
T X() const
Get the x value.
Definition: Vector3.hh:647
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix3.hh:417
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:223
static const Matrix3< T > Identity
Identity matrix.
Definition: Matrix3.hh:41
Matrix3< T > & operator=(const Quaternion< T > &_q)
Set the matrix3 from a quaternion.
Definition: Matrix3.hh:382
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: Helpers.hh:222
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition: Matrix3.hh:435
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:198
Matrix3()
Constructor.
Definition: Matrix3.hh:47
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:114
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix3< T > &_m)
Stream extraction operator.
Definition: Matrix3.hh:509
static const Matrix3< T > Zero
Zero matrix.
Definition: Matrix3.hh:44
Definition: Angle.hh:39
void Transpose()
Transpose this matrix.
Definition: Matrix3.hh:469
A 3x3 matrix class.
Definition: Matrix3.hh:38
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition: Matrix3.hh:409
Matrix3< T > & operator=(const Matrix3< T > &_mat)
Equal operator.
Definition: Matrix3.hh:223
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:395
Matrix3< T > operator *(const T &_s) const
returns the element wise scalar multiplication
Definition: Matrix3.hh:260
Matrix3< int > Matrix3i
Definition: Matrix3.hh:541
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix3< T > &_m)
Stream insertion operator.
Definition: Matrix3.hh:490
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:478