Gazebo Math

API Reference

7.5.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 gz::math
29 {
30  // Inline bracket to help doxygen filtering.
31  inline namespace GZ_MATH_VERSION_NAMESPACE {
32  //
33  template <typename T> class Quaternion;
34 
91  template<typename T>
92  class Matrix3
93  {
96  public: static const Matrix3<T> &Identity;
97 
100  public: static const Matrix3<T> &Zero;
101 
103  public: Matrix3()
104  {
105  std::memset(this->data, 0, sizeof(this->data[0][0])*9);
106  }
107 
110  public: Matrix3(const Matrix3<T> &_m) = default;
111 
122  public: constexpr Matrix3(T _v00, T _v01, T _v02,
123  T _v10, T _v11, T _v12,
124  T _v20, T _v21, T _v22)
125  : data{{_v00, _v01, _v02},
126  {_v10, _v11, _v12},
127  {_v20, _v21, _v22}}
128  {
129  }
130 
133  public: explicit Matrix3(const Quaternion<T> &_q)
134  {
135  Quaternion<T> qt = _q;
136  qt.Normalize();
137  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
138  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
139  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
140  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
141  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
142  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
143  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
144  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
145  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
146  }
147 
149  public: ~Matrix3() = default;
150 
155  public: void Set(size_t _row, size_t _col, T _v)
156  {
157  this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
158  [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)] = _v;
159  }
160 
171  public: void Set(T _v00, T _v01, T _v02,
172  T _v10, T _v11, T _v12,
173  T _v20, T _v21, T _v22)
174  {
175  this->data[0][0] = _v00;
176  this->data[0][1] = _v01;
177  this->data[0][2] = _v02;
178  this->data[1][0] = _v10;
179  this->data[1][1] = _v11;
180  this->data[1][2] = _v12;
181  this->data[2][0] = _v20;
182  this->data[2][1] = _v21;
183  this->data[2][2] = _v22;
184  }
185 
192  public: void GZ_DEPRECATED(7) Axes(const Vector3<T> &_xAxis,
193  const Vector3<T> &_yAxis,
194  const Vector3<T> &_zAxis)
195  {
196  this->SetAxes(_xAxis, _yAxis, _zAxis);
197  }
198 
203  public: void SetAxes(const Vector3<T> &_xAxis,
204  const Vector3<T> &_yAxis,
205  const Vector3<T> &_zAxis)
206  {
207  this->SetCol(0, _xAxis);
208  this->SetCol(1, _yAxis);
209  this->SetCol(2, _zAxis);
210  }
211 
216  public: void GZ_DEPRECATED(7) Axis(const Vector3<T> &_axis, T _angle)
217  {
218  this->SetFromAxisAngle(_axis, _angle);
219  }
220 
224  public: void SetFromAxisAngle(const Vector3<T> &_axis, T _angle)
225  {
226  T c = cos(_angle);
227  T s = sin(_angle);
228  T C = 1-c;
229 
230  this->data[0][0] = _axis.X()*_axis.X()*C + c;
231  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
232  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
233 
234  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
235  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
236  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
237 
238  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
239  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
240  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
241  }
242 
250  public: void GZ_DEPRECATED(7) From2Axes(
251  const Vector3<T> &_v1, const Vector3<T> &_v2)
252  {
253  this->SetFrom2Axes(_v1, _v2);
254  }
255 
262  public: void SetFrom2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
263  {
264  const T _v1LengthSquared = _v1.SquaredLength();
265  if (_v1LengthSquared <= 0.0)
266  {
267  // zero vector - we can't handle this
268  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
269  return;
270  }
271 
272  const T _v2LengthSquared = _v2.SquaredLength();
273  if (_v2LengthSquared <= 0.0)
274  {
275  // zero vector - we can't handle this
276  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
277  return;
278  }
279 
280  const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
281  if (fabs(dot - 1.0) <= 1e-6)
282  {
283  // the vectors are parallel
284  this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
285  return;
286  }
287  else if (fabs(dot + 1.0) <= 1e-6)
288  {
289  // the vectors are opposite
290  this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
291  return;
292  }
293 
294  const Vector3<T> cross = _v1.Cross(_v2).Normalize();
295 
296  this->SetFromAxisAngle(cross, acos(dot));
297  }
298 
304  public: void GZ_DEPRECATED(7) Col(unsigned int _c, const Vector3<T> &_v)
305  {
306  this->SetCol(_c, _v);
307  }
308 
313  public: void SetCol(unsigned int _c, const Vector3<T> &_v)
314  {
315  unsigned int c = clamp(_c, 0u, 2u);
316 
317  this->data[0][c] = _v.X();
318  this->data[1][c] = _v.Y();
319  this->data[2][c] = _v.Z();
320  }
321 
325  public: Matrix3<T> &operator=(const Matrix3<T> &_mat) = default;
326 
330  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
331  {
332  return Matrix3<T>(
333  this->data[0][0] - _m(0, 0),
334  this->data[0][1] - _m(0, 1),
335  this->data[0][2] - _m(0, 2),
336  this->data[1][0] - _m(1, 0),
337  this->data[1][1] - _m(1, 1),
338  this->data[1][2] - _m(1, 2),
339  this->data[2][0] - _m(2, 0),
340  this->data[2][1] - _m(2, 1),
341  this->data[2][2] - _m(2, 2));
342  }
343 
347  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
348  {
349  return Matrix3<T>(
350  this->data[0][0]+_m(0, 0),
351  this->data[0][1]+_m(0, 1),
352  this->data[0][2]+_m(0, 2),
353  this->data[1][0]+_m(1, 0),
354  this->data[1][1]+_m(1, 1),
355  this->data[1][2]+_m(1, 2),
356  this->data[2][0]+_m(2, 0),
357  this->data[2][1]+_m(2, 1),
358  this->data[2][2]+_m(2, 2));
359  }
360 
364  public: Matrix3<T> operator*(const T &_s) const
365  {
366  return Matrix3<T>(
367  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
368  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
369  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
370  }
371 
375  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
376  {
377  return Matrix3<T>(
378  // first row
379  this->data[0][0]*_m(0, 0)+
380  this->data[0][1]*_m(1, 0)+
381  this->data[0][2]*_m(2, 0),
382 
383  this->data[0][0]*_m(0, 1)+
384  this->data[0][1]*_m(1, 1)+
385  this->data[0][2]*_m(2, 1),
386 
387  this->data[0][0]*_m(0, 2)+
388  this->data[0][1]*_m(1, 2)+
389  this->data[0][2]*_m(2, 2),
390 
391  // second row
392  this->data[1][0]*_m(0, 0)+
393  this->data[1][1]*_m(1, 0)+
394  this->data[1][2]*_m(2, 0),
395 
396  this->data[1][0]*_m(0, 1)+
397  this->data[1][1]*_m(1, 1)+
398  this->data[1][2]*_m(2, 1),
399 
400  this->data[1][0]*_m(0, 2)+
401  this->data[1][1]*_m(1, 2)+
402  this->data[1][2]*_m(2, 2),
403 
404  // third row
405  this->data[2][0]*_m(0, 0)+
406  this->data[2][1]*_m(1, 0)+
407  this->data[2][2]*_m(2, 0),
408 
409  this->data[2][0]*_m(0, 1)+
410  this->data[2][1]*_m(1, 1)+
411  this->data[2][2]*_m(2, 1),
412 
413  this->data[2][0]*_m(0, 2)+
414  this->data[2][1]*_m(1, 2)+
415  this->data[2][2]*_m(2, 2));
416  }
417 
422  public: Vector3<T> operator*(const Vector3<T> &_vec) const
423  {
424  return Vector3<T>(
425  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
426  this->data[0][2]*_vec.Z(),
427  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
428  this->data[1][2]*_vec.Z(),
429  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
430  this->data[2][2]*_vec.Z());
431  }
432 
437  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
438  {
439  return _m * _s;
440  }
441 
448  public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
449  const Matrix3<T> &_m)
450  {
451  return Vector3<T>(
452  _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
453  _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
454  _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
455  }
456 
462  public: bool Equal(const Matrix3 &_m, const T &_tol) const
463  {
464  return equal<T>(this->data[0][0], _m(0, 0), _tol)
465  && equal<T>(this->data[0][1], _m(0, 1), _tol)
466  && equal<T>(this->data[0][2], _m(0, 2), _tol)
467  && equal<T>(this->data[1][0], _m(1, 0), _tol)
468  && equal<T>(this->data[1][1], _m(1, 1), _tol)
469  && equal<T>(this->data[1][2], _m(1, 2), _tol)
470  && equal<T>(this->data[2][0], _m(2, 0), _tol)
471  && equal<T>(this->data[2][1], _m(2, 1), _tol)
472  && equal<T>(this->data[2][2], _m(2, 2), _tol);
473  }
474 
478  public: bool operator==(const Matrix3<T> &_m) const
479  {
480  return this->Equal(_m, static_cast<T>(1e-6));
481  }
482 
486  public: Matrix3<T> &operator=(const Quaternion<T> &_q)
487  {
488  return *this = Matrix3<T>(_q);
489  }
490 
494  public: bool operator!=(const Matrix3<T> &_m) const
495  {
496  return !(*this == _m);
497  }
498 
503  public: inline T operator()(size_t _row, size_t _col) const
504  {
505  return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
506  [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
507  }
508 
513  public: inline T &operator()(size_t _row, size_t _col)
514  {
515  return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
516  [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
517  }
518 
521  public: T Determinant() const
522  {
523  T t0 = this->data[2][2]*this->data[1][1]
524  - this->data[2][1]*this->data[1][2];
525 
526  T t1 = -(this->data[2][2]*this->data[1][0]
527  -this->data[2][0]*this->data[1][2]);
528 
529  T t2 = this->data[2][1]*this->data[1][0]
530  - this->data[2][0]*this->data[1][1];
531 
532  return t0 * this->data[0][0]
533  + t1 * this->data[0][1]
534  + t2 * this->data[0][2];
535  }
536 
539  public: Matrix3<T> Inverse() const
540  {
541  T t0 = this->data[2][2]*this->data[1][1] -
542  this->data[2][1]*this->data[1][2];
543 
544  T t1 = -(this->data[2][2]*this->data[1][0] -
545  this->data[2][0]*this->data[1][2]);
546 
547  T t2 = this->data[2][1]*this->data[1][0] -
548  this->data[2][0]*this->data[1][1];
549 
550  T invDet = 1.0 / (t0 * this->data[0][0] +
551  t1 * this->data[0][1] +
552  t2 * this->data[0][2]);
553 
554  return invDet * Matrix3<T>(
555  t0,
556  - (this->data[2][2] * this->data[0][1] -
557  this->data[2][1] * this->data[0][2]),
558  + (this->data[1][2] * this->data[0][1] -
559  this->data[1][1] * this->data[0][2]),
560  t1,
561  + (this->data[2][2] * this->data[0][0] -
562  this->data[2][0] * this->data[0][2]),
563  - (this->data[1][2] * this->data[0][0] -
564  this->data[1][0] * this->data[0][2]),
565  t2,
566  - (this->data[2][1] * this->data[0][0] -
567  this->data[2][0] * this->data[0][1]),
568  + (this->data[1][1] * this->data[0][0] -
569  this->data[1][0] * this->data[0][1]));
570  }
571 
573  public: void Transpose()
574  {
575  std::swap(this->data[0][1], this->data[1][0]);
576  std::swap(this->data[0][2], this->data[2][0]);
577  std::swap(this->data[1][2], this->data[2][1]);
578  }
579 
582  public: Matrix3<T> Transposed() const
583  {
584  return Matrix3<T>(
585  this->data[0][0], this->data[1][0], this->data[2][0],
586  this->data[0][1], this->data[1][1], this->data[2][1],
587  this->data[0][2], this->data[1][2], this->data[2][2]);
588  }
589 
595  public: friend std::ostream &operator<<(
596  std::ostream &_out, const gz::math::Matrix3<T> &_m)
597  {
598  for (auto i : {0, 1, 2})
599  {
600  for (auto j : {0, 1, 2})
601  {
602  if (!(i == 0 && j == 0))
603  _out << " ";
604 
605  appendToStream(_out, _m(i, j));
606  }
607  }
608 
609  return _out;
610  }
611 
617  public: friend std::istream &operator>>(
618  std::istream &_in, gz::math::Matrix3<T> &_m)
619  {
620  // Skip white spaces
621  _in.setf(std::ios_base::skipws);
622  T d[9];
623  _in >> d[0] >> d[1] >> d[2]
624  >> d[3] >> d[4] >> d[5]
625  >> d[6] >> d[7] >> d[8];
626 
627  if (!_in.fail())
628  {
629  _m.Set(d[0], d[1], d[2],
630  d[3], d[4], d[5],
631  d[6], d[7], d[8]);
632  }
633  return _in;
634  }
635 
637  private: T data[3][3];
638  };
639 
640  namespace detail {
641 
642  template<typename T>
643  constexpr Matrix3<T> gMatrix3Identity(
644  1, 0, 0,
645  0, 1, 0,
646  0, 0, 1);
647 
648  template<typename T>
649  constexpr Matrix3<T> gMatrix3Zero(
650  0, 0, 0,
651  0, 0, 0,
652  0, 0, 0);
653 
654  } // namespace detail
655 
656  template<typename T>
657  const Matrix3<T> &Matrix3<T>::Identity = detail::gMatrix3Identity<T>;
658 
659  template<typename T>
660  const Matrix3<T> &Matrix3<T>::Zero = detail::gMatrix3Zero<T>;
661 
663  typedef Matrix3<int> Matrix3i;
664 
666  typedef Matrix3<double> Matrix3d;
667 
669  typedef Matrix3<float> Matrix3f;
670  } // namespace GZ_MATH_VERSION_NAMESPACE
671 } // namespace gz::math
672 #endif // GZ_MATH_MATRIX3_HH_