Gazebo Math

API Reference

6.15.1
gz/math/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 GZ_MATH_MATRIX3_HH_
18 #define GZ_MATH_MATRIX3_HH_
19 
20 #include <algorithm>
21 #include <cstring>
22 #include <utility>
23 #include <gz/math/Helpers.hh>
24 #include <gz/math/Vector3.hh>
25 #include <gz/math/Quaternion.hh>
26 #include <gz/math/config.hh>
27 
28 namespace ignition
29 {
30  namespace math
31  {
32  // Inline bracket to help doxygen filtering.
33  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
34  //
35  template <typename T> class Quaternion;
36 
39  template<typename T>
40  class Matrix3
41  {
43  public: static const Matrix3<T> Identity;
44 
46  public: static const Matrix3<T> Zero;
47 
49  public: Matrix3()
50  {
51  std::memset(this->data, 0, sizeof(this->data[0][0])*9);
52  }
53 
56  public: Matrix3(const Matrix3<T> &_m)
57  {
58  std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
59  }
60 
71  public: Matrix3(T _v00, T _v01, T _v02,
72  T _v10, T _v11, T _v12,
73  T _v20, T _v21, T _v22)
74  {
75  this->data[0][0] = _v00;
76  this->data[0][1] = _v01;
77  this->data[0][2] = _v02;
78  this->data[1][0] = _v10;
79  this->data[1][1] = _v11;
80  this->data[1][2] = _v12;
81  this->data[2][0] = _v20;
82  this->data[2][1] = _v21;
83  this->data[2][2] = _v22;
84  }
85 
88  public: explicit Matrix3(const Quaternion<T> &_q)
89  {
90  Quaternion<T> qt = _q;
91  qt.Normalize();
92  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
93  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
94  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
95  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
96  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
97  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
98  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
99  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
100  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
101  }
102 
104  public: virtual ~Matrix3() {}
105 
116  public: void Set(T _v00, T _v01, T _v02,
117  T _v10, T _v11, T _v12,
118  T _v20, T _v21, T _v22)
119  {
120  this->data[0][0] = _v00;
121  this->data[0][1] = _v01;
122  this->data[0][2] = _v02;
123  this->data[1][0] = _v10;
124  this->data[1][1] = _v11;
125  this->data[1][2] = _v12;
126  this->data[2][0] = _v20;
127  this->data[2][1] = _v21;
128  this->data[2][2] = _v22;
129  }
130 
135  public: void Axes(const Vector3<T> &_xAxis,
136  const Vector3<T> &_yAxis,
137  const Vector3<T> &_zAxis)
138  {
139  this->Col(0, _xAxis);
140  this->Col(1, _yAxis);
141  this->Col(2, _zAxis);
142  }
143 
147  public: void Axis(const Vector3<T> &_axis, T _angle)
148  {
149  T c = cos(_angle);
150  T s = sin(_angle);
151  T C = 1-c;
152 
153  this->data[0][0] = _axis.X()*_axis.X()*C + c;
154  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
155  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
156 
157  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
158  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
159  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
160 
161  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
162  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
163  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
164  }
165 
172  public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
173  {
174  const T _v1LengthSquared = _v1.SquaredLength();
175  if (_v1LengthSquared <= 0.0)
176  {
177  // zero vector - we can't handle this
178  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
179  return;
180  }
181 
182  const T _v2LengthSquared = _v2.SquaredLength();
183  if (_v2LengthSquared <= 0.0)
184  {
185  // zero vector - we can't handle this
186  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
187  return;
188  }
189 
190  const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
191  if (fabs(dot - 1.0) <= 1e-6)
192  {
193  // the vectors are parallel
194  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
195  return;
196  }
197  else if (fabs(dot + 1.0) <= 1e-6)
198  {
199  // the vectors are opposite
200  this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
201  return;
202  }
203 
204  const Vector3<T> cross = _v1.Cross(_v2).Normalize();
205 
206  this->Axis(cross, acos(dot));
207  }
208 
213  public: void Col(unsigned int _c, const Vector3<T> &_v)
214  {
215  unsigned int c = clamp(_c, 0u, 2u);
216 
217  this->data[0][c] = _v.X();
218  this->data[1][c] = _v.Y();
219  this->data[2][c] = _v.Z();
220  }
221 
225  public: Matrix3<T> &operator=(const Matrix3<T> &_mat)
226  {
227  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*9);
228  return *this;
229  }
230 
232  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
233  {
234  return Matrix3<T>(
235  this->data[0][0] - _m(0, 0),
236  this->data[0][1] - _m(0, 1),
237  this->data[0][2] - _m(0, 2),
238  this->data[1][0] - _m(1, 0),
239  this->data[1][1] - _m(1, 1),
240  this->data[1][2] - _m(1, 2),
241  this->data[2][0] - _m(2, 0),
242  this->data[2][1] - _m(2, 1),
243  this->data[2][2] - _m(2, 2));
244  }
245 
247  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
248  {
249  return Matrix3<T>(
250  this->data[0][0]+_m(0, 0),
251  this->data[0][1]+_m(0, 1),
252  this->data[0][2]+_m(0, 2),
253  this->data[1][0]+_m(1, 0),
254  this->data[1][1]+_m(1, 1),
255  this->data[1][2]+_m(1, 2),
256  this->data[2][0]+_m(2, 0),
257  this->data[2][1]+_m(2, 1),
258  this->data[2][2]+_m(2, 2));
259  }
260 
262  public: Matrix3<T> operator*(const T &_s) const
263  {
264  return Matrix3<T>(
265  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
266  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
267  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
268  }
269 
273  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
274  {
275  return Matrix3<T>(
276  // first row
277  this->data[0][0]*_m(0, 0)+
278  this->data[0][1]*_m(1, 0)+
279  this->data[0][2]*_m(2, 0),
280 
281  this->data[0][0]*_m(0, 1)+
282  this->data[0][1]*_m(1, 1)+
283  this->data[0][2]*_m(2, 1),
284 
285  this->data[0][0]*_m(0, 2)+
286  this->data[0][1]*_m(1, 2)+
287  this->data[0][2]*_m(2, 2),
288 
289  // second row
290  this->data[1][0]*_m(0, 0)+
291  this->data[1][1]*_m(1, 0)+
292  this->data[1][2]*_m(2, 0),
293 
294  this->data[1][0]*_m(0, 1)+
295  this->data[1][1]*_m(1, 1)+
296  this->data[1][2]*_m(2, 1),
297 
298  this->data[1][0]*_m(0, 2)+
299  this->data[1][1]*_m(1, 2)+
300  this->data[1][2]*_m(2, 2),
301 
302  // third row
303  this->data[2][0]*_m(0, 0)+
304  this->data[2][1]*_m(1, 0)+
305  this->data[2][2]*_m(2, 0),
306 
307  this->data[2][0]*_m(0, 1)+
308  this->data[2][1]*_m(1, 1)+
309  this->data[2][2]*_m(2, 1),
310 
311  this->data[2][0]*_m(0, 2)+
312  this->data[2][1]*_m(1, 2)+
313  this->data[2][2]*_m(2, 2));
314  }
315 
320  public: Vector3<T> operator*(const Vector3<T> &_vec) const
321  {
322  return Vector3<T>(
323  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
324  this->data[0][2]*_vec.Z(),
325  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
326  this->data[1][2]*_vec.Z(),
327  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
328  this->data[2][2]*_vec.Z());
329  }
330 
335  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
336  {
337  return _m * _s;
338  }
339 
346  public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
347  const Matrix3<T> &_m)
348  {
349  return Vector3<T>(
350  _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
351  _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
352  _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
353  }
354 
360  public: bool Equal(const Matrix3 &_m, const T &_tol) const
361  {
362  return equal<T>(this->data[0][0], _m(0, 0), _tol)
363  && equal<T>(this->data[0][1], _m(0, 1), _tol)
364  && equal<T>(this->data[0][2], _m(0, 2), _tol)
365  && equal<T>(this->data[1][0], _m(1, 0), _tol)
366  && equal<T>(this->data[1][1], _m(1, 1), _tol)
367  && equal<T>(this->data[1][2], _m(1, 2), _tol)
368  && equal<T>(this->data[2][0], _m(2, 0), _tol)
369  && equal<T>(this->data[2][1], _m(2, 1), _tol)
370  && equal<T>(this->data[2][2], _m(2, 2), _tol);
371  }
372 
376  public: bool operator==(const Matrix3<T> &_m) const
377  {
378  return this->Equal(_m, static_cast<T>(1e-6));
379  }
380 
384  public: Matrix3<T> &operator=(const Quaternion<T> &_q)
385  {
386  return *this = Matrix3<T>(_q);
387  }
388 
392  public: bool operator!=(const Matrix3<T> &_m) const
393  {
394  return !(*this == _m);
395  }
396 
401  public: inline const T &operator()(size_t _row, size_t _col) const
402  {
403  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
405  }
406 
411  public: inline T &operator()(size_t _row, size_t _col)
412  {
413  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
415  }
416 
419  public: T Determinant() const
420  {
421  T t0 = this->data[2][2]*this->data[1][1]
422  - this->data[2][1]*this->data[1][2];
423 
424  T t1 = -(this->data[2][2]*this->data[1][0]
425  -this->data[2][0]*this->data[1][2]);
426 
427  T t2 = this->data[2][1]*this->data[1][0]
428  - this->data[2][0]*this->data[1][1];
429 
430  return t0 * this->data[0][0]
431  + t1 * this->data[0][1]
432  + t2 * this->data[0][2];
433  }
434 
437  public: Matrix3<T> Inverse() const
438  {
439  T t0 = this->data[2][2]*this->data[1][1] -
440  this->data[2][1]*this->data[1][2];
441 
442  T t1 = -(this->data[2][2]*this->data[1][0] -
443  this->data[2][0]*this->data[1][2]);
444 
445  T t2 = this->data[2][1]*this->data[1][0] -
446  this->data[2][0]*this->data[1][1];
447 
448  T invDet = 1.0 / (t0 * this->data[0][0] +
449  t1 * this->data[0][1] +
450  t2 * this->data[0][2]);
451 
452  return invDet * Matrix3<T>(
453  t0,
454  - (this->data[2][2] * this->data[0][1] -
455  this->data[2][1] * this->data[0][2]),
456  + (this->data[1][2] * this->data[0][1] -
457  this->data[1][1] * this->data[0][2]),
458  t1,
459  + (this->data[2][2] * this->data[0][0] -
460  this->data[2][0] * this->data[0][2]),
461  - (this->data[1][2] * this->data[0][0] -
462  this->data[1][0] * this->data[0][2]),
463  t2,
464  - (this->data[2][1] * this->data[0][0] -
465  this->data[2][0] * this->data[0][1]),
466  + (this->data[1][1] * this->data[0][0] -
467  this->data[1][0] * this->data[0][1]));
468  }
469 
471  public: void Transpose()
472  {
473  std::swap(this->data[0][1], this->data[1][0]);
474  std::swap(this->data[0][2], this->data[2][0]);
475  std::swap(this->data[1][2], this->data[2][1]);
476  }
477 
480  public: Matrix3<T> Transposed() const
481  {
482  return Matrix3<T>(
483  this->data[0][0], this->data[1][0], this->data[2][0],
484  this->data[0][1], this->data[1][1], this->data[2][1],
485  this->data[0][2], this->data[1][2], this->data[2][2]);
486  }
487 
492  public: friend std::ostream &operator<<(
493  std::ostream &_out, const gz::math::Matrix3<T> &_m)
494  {
495  _out << precision(_m(0, 0), 6) << " "
496  << precision(_m(0, 1), 6) << " "
497  << precision(_m(0, 2), 6) << " "
498  << precision(_m(1, 0), 6) << " "
499  << precision(_m(1, 1), 6) << " "
500  << precision(_m(1, 2), 6) << " "
501  << precision(_m(2, 0), 6) << " "
502  << precision(_m(2, 1), 6) << " "
503  << precision(_m(2, 2), 6);
504 
505  return _out;
506  }
507 
512  public: friend std::istream &operator>>(
513  std::istream &_in, gz::math::Matrix3<T> &_m)
514  {
515  // Skip white spaces
516  _in.setf(std::ios_base::skipws);
517  T d[9];
518  _in >> d[0] >> d[1] >> d[2]
519  >> d[3] >> d[4] >> d[5]
520  >> d[6] >> d[7] >> d[8];
521 
522  if (!_in.fail())
523  {
524  _m.Set(d[0], d[1], d[2],
525  d[3], d[4], d[5],
526  d[6], d[7], d[8]);
527  }
528  return _in;
529  }
530 
532  private: T data[3][3];
533  };
534 
535  template<typename T>
536  const Matrix3<T> Matrix3<T>::Identity(
537  1, 0, 0,
538  0, 1, 0,
539  0, 0, 1);
540 
541  template<typename T>
542  const Matrix3<T> Matrix3<T>::Zero(
543  0, 0, 0,
544  0, 0, 0,
545  0, 0, 0);
546 
550  }
551  }
552 }
553 
554 #endif
Matrix3< double > Matrix3d
Definition: gz/math/Matrix3.hh:548
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition: gz/math/Matrix3.hh:411
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition: gz/math/Matrix3.hh:401
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
bool Equal(const Matrix3 &_m, const T &_tol) const
Equality test with tolerance.
Definition: gz/math/Matrix3.hh:360
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: gz/math/Matrix3.hh:172
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition: gz/math/Matrix3.hh:56
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition: gz/math/Matrix3.hh:392
static const Matrix3< T > Identity
Identity matrix.
Definition: gz/math/Matrix3.hh:43
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: gz/math/Vector3.hh:129
friend std::ostream & operator<<(std::ostream &_out, const Matrix3< T > &_m)
Stream insertion operator.
Definition: gz/math/Matrix3.hh:492
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
void Transpose()
Transpose this matrix.
Definition: gz/math/Matrix3.hh:471
A 3x3 matrix class.
Definition: gz/math/Matrix3.hh:40
friend Vector3< T > operator*(const Vector3< T > &_v, const Matrix3< T > &_m)
Matrix left multiplication operator for Vector3. Treats the Vector3 like a row vector multiplying the...
Definition: gz/math/Matrix3.hh:346
Matrix3< T > & operator=(const Matrix3< T > &_mat)
Equal operator. this = _mat.
Definition: gz/math/Matrix3.hh:225
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition: gz/math/Matrix3.hh:71
Matrix3< T > operator*(const T &_s) const
returns the element wise scalar multiplication
Definition: gz/math/Matrix3.hh:262
A quaternion class.
Definition: gz/math/Matrix3.hh:35
virtual ~Matrix3()
Desctructor.
Definition: gz/math/Matrix3.hh:104
void Normalize()
Normalize the quaternion.
Definition: gz/math/Quaternion.hh:224
Matrix3< int > Matrix3i
Definition: gz/math/Matrix3.hh:547
Matrix3< T > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition: gz/math/Matrix3.hh:232
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition: gz/math/Matrix3.hh:247
Matrix3< T > & operator=(const Quaternion< T > &_q)
Set the matrix3 from a quaternion.
Definition: gz/math/Matrix3.hh:384
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: gz/math/Helpers.hh:590
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: gz/math/Helpers.hh:233
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
const T & Y() const
Get the y component.
Definition: gz/math/Quaternion.hh:979
STL class.
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: gz/math/Helpers.hh:406
T setf(T... args)
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator with Vector3 on the right treated like a column vector.
Definition: gz/math/Matrix3.hh:320
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition: gz/math/Matrix3.hh:213
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
friend std::istream & operator>>(std::istream &_in, Matrix3< T > &_m)
Stream extraction operator.
Definition: gz/math/Matrix3.hh:512
const T & Z() const
Get the z component.
Definition: gz/math/Quaternion.hh:986
void Set(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Set values.
Definition: gz/math/Matrix3.hh:116
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition: gz/math/Matrix3.hh:376
T swap(T... args)
void Axes(const Vector3< T > &_xAxis, const Vector3< T > &_yAxis, const Vector3< T > &_zAxis)
Set the matrix from three axis (1 per column)
Definition: gz/math/Matrix3.hh:135
Matrix3()
Constructor.
Definition: gz/math/Matrix3.hh:49
Matrix3< float > Matrix3f
Definition: gz/math/Matrix3.hh:549
T fail(T... args)
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition: gz/math/Matrix3.hh:437
friend Matrix3< T > operator*(T _s, const Matrix3< T > &_m)
Matrix multiplication operator for scaling.
Definition: gz/math/Matrix3.hh:335
void Axis(const Vector3< T > &_axis, T _angle)
Set the matrix from an axis and angle.
Definition: gz/math/Matrix3.hh:147
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: gz/math/Helpers.hh:227
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: gz/math/Vector3.hh:205
const T & X() const
Get the x component.
Definition: gz/math/Quaternion.hh:972
T memcpy(T... args)
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition: gz/math/Matrix3.hh:88
Matrix3< T > operator*(const Matrix3< T > &_m) const
Matrix multiplication operator.
Definition: gz/math/Matrix3.hh:273
static const Matrix3< T > Zero
Zero matrix.
Definition: gz/math/Matrix3.hh:46
STL class.
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661
T Determinant() const
Return the determinant of the matrix.
Definition: gz/math/Matrix3.hh:419
const T & W() const
Get the w component.
Definition: gz/math/Quaternion.hh:965
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: gz/math/Matrix3.hh:480
T memset(T... args)
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: gz/math/Vector3.hh:195