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 #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 
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: explicit 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 
208  public: void Col(unsigned int _c, const Vector3<T> &_v)
209  {
210  unsigned int c = clamp(_c, 0u, 2u);
211 
212  this->data[0][c] = _v.X();
213  this->data[1][c] = _v.Y();
214  this->data[2][c] = _v.Z();
215  }
216 
220  public: Matrix3<T> &operator=(const Matrix3<T> &_mat)
221  {
222  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*9);
223  return *this;
224  }
225 
227  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
228  {
229  return Matrix3<T>(
230  this->data[0][0] - _m(0, 0),
231  this->data[0][1] - _m(0, 1),
232  this->data[0][2] - _m(0, 2),
233  this->data[1][0] - _m(1, 0),
234  this->data[1][1] - _m(1, 1),
235  this->data[1][2] - _m(1, 2),
236  this->data[2][0] - _m(2, 0),
237  this->data[2][1] - _m(2, 1),
238  this->data[2][2] - _m(2, 2));
239  }
240 
242  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
243  {
244  return Matrix3<T>(
245  this->data[0][0]+_m(0, 0),
246  this->data[0][1]+_m(0, 1),
247  this->data[0][2]+_m(0, 2),
248  this->data[1][0]+_m(1, 0),
249  this->data[1][1]+_m(1, 1),
250  this->data[1][2]+_m(1, 2),
251  this->data[2][0]+_m(2, 0),
252  this->data[2][1]+_m(2, 1),
253  this->data[2][2]+_m(2, 2));
254  }
255 
257  public: Matrix3<T> operator*(const T &_s) const
258  {
259  return Matrix3<T>(
260  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
261  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
262  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
263  }
264 
268  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
269  {
270  return Matrix3<T>(
271  // first row
272  this->data[0][0]*_m(0, 0)+
273  this->data[0][1]*_m(1, 0)+
274  this->data[0][2]*_m(2, 0),
275 
276  this->data[0][0]*_m(0, 1)+
277  this->data[0][1]*_m(1, 1)+
278  this->data[0][2]*_m(2, 1),
279 
280  this->data[0][0]*_m(0, 2)+
281  this->data[0][1]*_m(1, 2)+
282  this->data[0][2]*_m(2, 2),
283 
284  // second row
285  this->data[1][0]*_m(0, 0)+
286  this->data[1][1]*_m(1, 0)+
287  this->data[1][2]*_m(2, 0),
288 
289  this->data[1][0]*_m(0, 1)+
290  this->data[1][1]*_m(1, 1)+
291  this->data[1][2]*_m(2, 1),
292 
293  this->data[1][0]*_m(0, 2)+
294  this->data[1][1]*_m(1, 2)+
295  this->data[1][2]*_m(2, 2),
296 
297  // third row
298  this->data[2][0]*_m(0, 0)+
299  this->data[2][1]*_m(1, 0)+
300  this->data[2][2]*_m(2, 0),
301 
302  this->data[2][0]*_m(0, 1)+
303  this->data[2][1]*_m(1, 1)+
304  this->data[2][2]*_m(2, 1),
305 
306  this->data[2][0]*_m(0, 2)+
307  this->data[2][1]*_m(1, 2)+
308  this->data[2][2]*_m(2, 2));
309  }
310 
315  public: Vector3<T> operator*(const Vector3<T> &_vec) const
316  {
317  return Vector3<T>(
318  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
319  this->data[0][2]*_vec.Z(),
320  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
321  this->data[1][2]*_vec.Z(),
322  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
323  this->data[2][2]*_vec.Z());
324  }
325 
330  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
331  {
332  return _m * _s;
333  }
334 
341  public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
342  const Matrix3<T> &_m)
343  {
344  return Vector3<T>(
345  _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
346  _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
347  _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
348  }
349 
355  public: bool Equal(const Matrix3 &_m, const T &_tol) const
356  {
357  return equal<T>(this->data[0][0], _m(0, 0), _tol)
358  && equal<T>(this->data[0][1], _m(0, 1), _tol)
359  && equal<T>(this->data[0][2], _m(0, 2), _tol)
360  && equal<T>(this->data[1][0], _m(1, 0), _tol)
361  && equal<T>(this->data[1][1], _m(1, 1), _tol)
362  && equal<T>(this->data[1][2], _m(1, 2), _tol)
363  && equal<T>(this->data[2][0], _m(2, 0), _tol)
364  && equal<T>(this->data[2][1], _m(2, 1), _tol)
365  && equal<T>(this->data[2][2], _m(2, 2), _tol);
366  }
367 
371  public: bool operator==(const Matrix3<T> &_m) const
372  {
373  return this->Equal(_m, static_cast<T>(1e-6));
374  }
375 
379  public: Matrix3<T> &operator=(const Quaternion<T> &_q)
380  {
381  return *this = Matrix3<T>(_q);
382  }
383 
387  public: bool operator!=(const Matrix3<T> &_m) const
388  {
389  return !(*this == _m);
390  }
391 
396  public: inline const T &operator()(size_t _row, size_t _col) const
397  {
398  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
400  }
401 
406  public: inline T &operator()(size_t _row, size_t _col)
407  {
408  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
410  }
411 
414  public: T Determinant() const
415  {
416  T t0 = this->data[2][2]*this->data[1][1]
417  - this->data[2][1]*this->data[1][2];
418 
419  T t1 = -(this->data[2][2]*this->data[1][0]
420  -this->data[2][0]*this->data[1][2]);
421 
422  T t2 = this->data[2][1]*this->data[1][0]
423  - this->data[2][0]*this->data[1][1];
424 
425  return t0 * this->data[0][0]
426  + t1 * this->data[0][1]
427  + t2 * this->data[0][2];
428  }
429 
432  public: Matrix3<T> Inverse() const
433  {
434  T t0 = this->data[2][2]*this->data[1][1] -
435  this->data[2][1]*this->data[1][2];
436 
437  T t1 = -(this->data[2][2]*this->data[1][0] -
438  this->data[2][0]*this->data[1][2]);
439 
440  T t2 = this->data[2][1]*this->data[1][0] -
441  this->data[2][0]*this->data[1][1];
442 
443  T invDet = 1.0 / (t0 * this->data[0][0] +
444  t1 * this->data[0][1] +
445  t2 * this->data[0][2]);
446 
447  return invDet * Matrix3<T>(
448  t0,
449  - (this->data[2][2] * this->data[0][1] -
450  this->data[2][1] * this->data[0][2]),
451  + (this->data[1][2] * this->data[0][1] -
452  this->data[1][1] * this->data[0][2]),
453  t1,
454  + (this->data[2][2] * this->data[0][0] -
455  this->data[2][0] * this->data[0][2]),
456  - (this->data[1][2] * this->data[0][0] -
457  this->data[1][0] * this->data[0][2]),
458  t2,
459  - (this->data[2][1] * this->data[0][0] -
460  this->data[2][0] * this->data[0][1]),
461  + (this->data[1][1] * this->data[0][0] -
462  this->data[1][0] * this->data[0][1]));
463  }
464 
466  public: void Transpose()
467  {
468  std::swap(this->data[0][1], this->data[1][0]);
469  std::swap(this->data[0][2], this->data[2][0]);
470  std::swap(this->data[1][2], this->data[2][1]);
471  }
472 
475  public: Matrix3<T> Transposed() const
476  {
477  return Matrix3<T>(
478  this->data[0][0], this->data[1][0], this->data[2][0],
479  this->data[0][1], this->data[1][1], this->data[2][1],
480  this->data[0][2], this->data[1][2], this->data[2][2]);
481  }
482 
487  public: friend std::ostream &operator<<(
488  std::ostream &_out, const ignition::math::Matrix3<T> &_m)
489  {
490  _out << precision(_m(0, 0), 6) << " "
491  << precision(_m(0, 1), 6) << " "
492  << precision(_m(0, 2), 6) << " "
493  << precision(_m(1, 0), 6) << " "
494  << precision(_m(1, 1), 6) << " "
495  << precision(_m(1, 2), 6) << " "
496  << precision(_m(2, 0), 6) << " "
497  << precision(_m(2, 1), 6) << " "
498  << precision(_m(2, 2), 6);
499 
500  return _out;
501  }
506  public: friend std::istream &operator>>(
507  std::istream &_in, ignition::math::Matrix3<T> &_m)
508  {
509  // Skip white spaces
510  _in.setf(std::ios_base::skipws);
511  T d[9];
512  _in >> d[0] >> d[1] >> d[2]
513  >> d[3] >> d[4] >> d[5]
514  >> d[6] >> d[7] >> d[8];
515 
516  _m.Set(d[0], d[1], d[2],
517  d[3], d[4], d[5],
518  d[6], d[7], d[8]);
519  return _in;
520  }
521 
523  private: T data[3][3];
524  };
525 
526  template<typename T>
528  1, 0, 0,
529  0, 1, 0,
530  0, 0, 1);
531 
532  template<typename T>
534  0, 0, 0,
535  0, 0, 0,
536  0, 0, 0);
537 
541  }
542 }
543 
544 #endif
const T & W() const
Get the w component.
Definition: Quaternion.hh:935
T X() const
Get the x value.
Definition: Vector3.hh:635
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix3< T > &_m)
Stream extraction operator.
Definition: Matrix3.hh:506
const T & Z() const
Get the z component.
Definition: Quaternion.hh:956
Matrix3< double > Matrix3d
Definition: Matrix3.hh:539
Matrix3< T > operator*(const T &_s) const
returns the element wise scalar multiplication
Definition: Matrix3.hh:257
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:552
Matrix3< int > Matrix3i
Definition: Matrix3.hh:538
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition: Matrix3.hh:51
Matrix3()
Constructor.
Definition: Matrix3.hh:44
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition: Matrix3.hh:406
friend Vector3< T > operator*(const Vector3< T > &_v, const Matrix3< T > &_m)
Matrix left multiplication operator for Vector3.
Definition: Matrix3.hh:341
Matrix3< T > & operator=(const Matrix3< T > &_mat)
Equal operator.
Definition: Matrix3.hh:220
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
Matrix3< T > operator*(const Matrix3< T > &_m) const
Matrix multiplication operator.
Definition: Matrix3.hh:268
static const Matrix3< T > Zero
Zero matrix.
Definition: Matrix3.hh:41
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition: Matrix3.hh:432
friend Matrix3< T > operator*(T _s, const Matrix3< T > &_m)
Matrix multiplication operator for scaling.
Definition: Matrix3.hh:330
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:195
virtual ~Matrix3()
Desctructor.
Definition: Matrix3.hh:99
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:120
bool Equal(const Matrix3 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix3.hh:355
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition: Matrix3.hh:208
A 3x3 matrix class.
Definition: Matrix3.hh:35
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition: Matrix3.hh:371
Matrix3< float > Matrix3f
Definition: Matrix3.hh:540
const T & Y() const
Get the y component.
Definition: Quaternion.hh:949
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:475
T Y() const
Get the y value.
Definition: Vector3.hh:642
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator with Vector3 on the right treated like a column vector.
Definition: Matrix3.hh:315
Matrix3< T > & operator=(const Quaternion< T > &_q)
Set the matrix3 from a quaternion.
Definition: Matrix3.hh:379
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition: Matrix3.hh:387
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: Helpers.hh:219
const T & X() const
Get the x component.
Definition: Quaternion.hh:942
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition: Matrix3.hh:396
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
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
T Z() const
Get the z value.
Definition: Vector3.hh:649
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition: Matrix3.hh:242
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:185
void Transpose()
Transpose this matrix.
Definition: Matrix3.hh:466
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix3.hh:414
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix3< T > &_m)
Stream insertion operator.
Definition: Matrix3.hh:487
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
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition: Matrix3.hh:83
Definition: Angle.hh:38
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 > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition: Matrix3.hh:227
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition: Matrix3.hh:66
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392