Matrix6.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2022 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef GZ_MATH_MATRIX6_HH_
#define GZ_MATH_MATRIX6_HH_
 
#include <utility>
#include <gz/math/config.hh>
#include <gz/math/Helpers.hh>
#include <gz/math/Matrix3.hh>
 
namespace gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_MATH_VERSION_NAMESPACE {
  //
  template<typename T>
  class Matrix6
  {
    public: enum Matrix6Corner
    {
      TOP_LEFT = 0,
 
      TOP_RIGHT = 1,
 
      BOTTOM_LEFT = 2,
 
      BOTTOM_RIGHT = 3
    };
 
    public: static constexpr std::size_t MatrixSize{6};
 
    public: static const Matrix6<T> &Identity;
 
    public: static const Matrix6<T> &Zero;
 
    public: Matrix6()
    {
      memset(this->data, 0, sizeof(this->data[0][0])*MatrixSize*MatrixSize);
    }
 
    public: constexpr Matrix6(T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
                              T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
                              T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
                              T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
                              T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
                              T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
    : data{{_v00, _v01, _v02, _v03, _v04, _v05},
           {_v10, _v11, _v12, _v13, _v14, _v15},
           {_v20, _v21, _v22, _v23, _v24, _v25},
           {_v30, _v31, _v32, _v33, _v34, _v35},
           {_v40, _v41, _v42, _v43, _v44, _v45},
           {_v50, _v51, _v52, _v53, _v54, _v55}}
    {
    }
 
    public: bool SetValue(size_t _row, size_t _col, T _v)
    {
      if (_row < MatrixSize && _col < MatrixSize)
      {
        this->data[_row][_col] = _v;
        return true;
      }
      return false;
    }
 
    public: void Set(
        T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
        T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
        T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
        T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
        T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
        T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
    {
      this->data[0][0] = _v00;
      this->data[0][1] = _v01;
      this->data[0][2] = _v02;
      this->data[0][3] = _v03;
      this->data[0][4] = _v04;
      this->data[0][5] = _v05;
 
      this->data[1][0] = _v10;
      this->data[1][1] = _v11;
      this->data[1][2] = _v12;
      this->data[1][3] = _v13;
      this->data[1][4] = _v14;
      this->data[1][5] = _v15;
 
      this->data[2][0] = _v20;
      this->data[2][1] = _v21;
      this->data[2][2] = _v22;
      this->data[2][3] = _v23;
      this->data[2][4] = _v24;
      this->data[2][5] = _v25;
 
      this->data[3][0] = _v30;
      this->data[3][1] = _v31;
      this->data[3][2] = _v32;
      this->data[3][3] = _v33;
      this->data[3][4] = _v34;
      this->data[3][5] = _v35;
 
      this->data[4][0] = _v40;
      this->data[4][1] = _v41;
      this->data[4][2] = _v42;
      this->data[4][3] = _v43;
      this->data[4][4] = _v44;
      this->data[4][5] = _v45;
 
      this->data[5][0] = _v50;
      this->data[5][1] = _v51;
      this->data[5][2] = _v52;
      this->data[5][3] = _v53;
      this->data[5][4] = _v54;
      this->data[5][5] = _v55;
    }
 
    public: void Transpose()
    {
      std::swap(this->data[0][1], this->data[1][0]);
      std::swap(this->data[0][2], this->data[2][0]);
      std::swap(this->data[0][3], this->data[3][0]);
      std::swap(this->data[0][4], this->data[4][0]);
      std::swap(this->data[0][5], this->data[5][0]);
      std::swap(this->data[1][2], this->data[2][1]);
      std::swap(this->data[1][3], this->data[3][1]);
      std::swap(this->data[1][4], this->data[4][1]);
      std::swap(this->data[1][5], this->data[5][1]);
      std::swap(this->data[2][3], this->data[3][2]);
      std::swap(this->data[2][4], this->data[4][2]);
      std::swap(this->data[2][5], this->data[5][2]);
      std::swap(this->data[3][4], this->data[4][3]);
      std::swap(this->data[3][5], this->data[5][3]);
      std::swap(this->data[4][5], this->data[5][4]);
    }
 
    public: Matrix6<T> Transposed() const
    {
      return Matrix6<T>(
          this->data[0][0],
          this->data[1][0],
          this->data[2][0],
          this->data[3][0],
          this->data[4][0],
          this->data[5][0],
 
          this->data[0][1],
          this->data[1][1],
          this->data[2][1],
          this->data[3][1],
          this->data[4][1],
          this->data[5][1],
 
          this->data[0][2],
          this->data[1][2],
          this->data[2][2],
          this->data[3][2],
          this->data[4][2],
          this->data[5][2],
 
          this->data[0][3],
          this->data[1][3],
          this->data[2][3],
          this->data[3][3],
          this->data[4][3],
          this->data[5][3],
 
          this->data[0][4],
          this->data[1][4],
          this->data[2][4],
          this->data[3][4],
          this->data[4][4],
          this->data[5][4],
 
          this->data[0][5],
          this->data[1][5],
          this->data[2][5],
          this->data[3][5],
          this->data[4][5],
          this->data[5][5]);
    }
 
    public: Matrix6<T> operator*=(const Matrix6<T> &_m2)
    {
      (*this) = (*this) * _m2;
      return *this;
    }
 
    public: Matrix6<T> operator*(const Matrix6<T> &_m2) const
    {
      auto el = [&](size_t _row, size_t _col) -> T
      {
        T result = static_cast<T>(0);
        for (size_t i = 0; i < MatrixSize; ++i)
          result += this->data[_row][i] * _m2(i, _col);
        return result;
      };
      return Matrix6<T>(
          el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
          el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
          el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
          el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
          el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
          el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
    }
 
    public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
    {
      (*this) = (*this) + _m2;
      return *this;
    }
 
    public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
    {
      auto el = [&](size_t _row, size_t _col) -> T
      {
        return this->data[_row][_col] + _m2(_row, _col);
      };
      return Matrix6<T>(
          el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
          el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
          el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
          el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
          el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
          el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
    }
 
   public: inline const T &operator()(const size_t _row,
               const size_t _col) const
   {
     return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)][
                       clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
   }
 
   public: inline T &operator()(const size_t _row, const size_t _col)
   {
     return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)]
                      [clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
   }
 
   public: Matrix3<T> Submatrix(Matrix6Corner _corner) const
   {
     size_t row = 0;
     size_t col = 0;
     if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
     {
       row = 3;
     }
     if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
     {
       col = 3;
     }
     return {this->data[row + 0][col + 0],
             this->data[row + 0][col + 1],
             this->data[row + 0][col + 2],
             this->data[row + 1][col + 0],
             this->data[row + 1][col + 1],
             this->data[row + 1][col + 2],
             this->data[row + 2][col + 0],
             this->data[row + 2][col + 1],
             this->data[row + 2][col + 2]};
   }
 
   public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3<T> &_mat)
   {
     size_t row = 0;
     size_t col = 0;
     if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
     {
       row = 3;
     }
     if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
     {
       col = 3;
     }
     for (size_t r = 0; r < 3; ++r)
     {
       for (size_t c = 0; c < 3; ++c)
       {
         this->data[row + r][col + c] = _mat(r, c);
       }
     }
   }
 
    public: bool Equal(const Matrix6 &_m, const T &_tol) const
    {
      return equal<T>(this->data[0][0], _m(0, 0), _tol)
          && equal<T>(this->data[0][1], _m(0, 1), _tol)
          && equal<T>(this->data[0][2], _m(0, 2), _tol)
          && equal<T>(this->data[0][3], _m(0, 3), _tol)
          && equal<T>(this->data[0][4], _m(0, 4), _tol)
          && equal<T>(this->data[0][5], _m(0, 5), _tol)
          && equal<T>(this->data[1][0], _m(1, 0), _tol)
          && equal<T>(this->data[1][1], _m(1, 1), _tol)
          && equal<T>(this->data[1][2], _m(1, 2), _tol)
          && equal<T>(this->data[1][3], _m(1, 3), _tol)
          && equal<T>(this->data[1][4], _m(1, 4), _tol)
          && equal<T>(this->data[1][5], _m(1, 5), _tol)
          && equal<T>(this->data[2][0], _m(2, 0), _tol)
          && equal<T>(this->data[2][1], _m(2, 1), _tol)
          && equal<T>(this->data[2][2], _m(2, 2), _tol)
          && equal<T>(this->data[2][3], _m(2, 3), _tol)
          && equal<T>(this->data[2][4], _m(2, 4), _tol)
          && equal<T>(this->data[2][5], _m(2, 5), _tol)
          && equal<T>(this->data[3][0], _m(3, 0), _tol)
          && equal<T>(this->data[3][1], _m(3, 1), _tol)
          && equal<T>(this->data[3][2], _m(3, 2), _tol)
          && equal<T>(this->data[3][3], _m(3, 3), _tol)
          && equal<T>(this->data[3][4], _m(3, 4), _tol)
          && equal<T>(this->data[3][5], _m(3, 5), _tol)
          && equal<T>(this->data[4][0], _m(4, 0), _tol)
          && equal<T>(this->data[4][1], _m(4, 1), _tol)
          && equal<T>(this->data[4][2], _m(4, 2), _tol)
          && equal<T>(this->data[4][3], _m(4, 3), _tol)
          && equal<T>(this->data[4][4], _m(4, 4), _tol)
          && equal<T>(this->data[4][5], _m(4, 5), _tol)
          && equal<T>(this->data[5][0], _m(5, 0), _tol)
          && equal<T>(this->data[5][1], _m(5, 1), _tol)
          && equal<T>(this->data[5][2], _m(5, 2), _tol)
          && equal<T>(this->data[5][3], _m(5, 3), _tol)
          && equal<T>(this->data[5][4], _m(5, 4), _tol)
          && equal<T>(this->data[5][5], _m(5, 5), _tol);
    }
 
    public: bool operator==(const Matrix6<T> &_m) const
    {
      return this->Equal(_m, static_cast<T>(1e-6));
    }
 
    public: bool operator!=(const Matrix6<T> &_m) const
    {
      return !(*this == _m);
    }
 
    public: friend std::ostream &operator<<(
                std::ostream &_out, const gz::math::Matrix6<T> &_m)
    {
     for (auto i : {0, 1, 2, 3, 4, 5})
      {
        for (auto j : {0, 1, 2, 3, 4, 5})
        {
          if (!(i == 0 && j == 0))
            _out << " ";
 
          appendToStream(_out, _m(i, j));
        }
      }
 
      return _out;
    }
 
    public: friend std::istream &operator>>(
                std::istream &_in, gz::math::Matrix6<T> &_m)
    {
      // Skip white spaces
      _in.setf(std::ios_base::skipws);
      T d[36];
      _in >> d[0] >> d[1] >> d[2] >> d[3] >> d[4] >> d[5]
          >> d[6] >> d[7] >> d[8] >> d[9] >> d[10] >> d[11]
          >> d[12] >> d[13] >> d[14] >> d[15] >> d[16] >> d[17]
          >> d[18] >> d[19] >> d[20] >> d[21] >> d[22] >> d[23]
          >> d[24] >> d[25] >> d[26] >> d[27] >> d[28] >> d[29]
          >> d[30] >> d[31] >> d[32] >> d[33] >> d[34] >> d[35];
 
      if (!_in.fail())
      {
        _m.Set(d[0], d[1], d[2], d[3], d[4], d[5],
               d[6], d[7], d[8], d[9], d[10], d[11],
               d[12], d[13], d[14], d[15], d[16], d[17],
               d[18], d[19], d[20], d[21], d[22], d[23],
               d[24], d[25], d[26], d[27], d[28], d[29],
               d[30], d[31], d[32], d[33], d[34], d[35]);
      }
      return _in;
    }
 
    private: T data[MatrixSize][MatrixSize];
  };
 
  namespace detail {
 
    template<typename T>
    constexpr Matrix6<T> gMatrix6Identity(
        1, 0, 0, 0, 0, 0,
        0, 1, 0, 0, 0, 0,
        0, 0, 1, 0, 0, 0,
        0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 1);
 
    template<typename T>
    constexpr Matrix6<T> gMatrix6Zero(
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0);
 
  }  // namespace detail
 
  template<typename T>
  const Matrix6<T> &Matrix6<T>::Identity = detail::gMatrix6Identity<T>;
 
  template<typename T>
  const Matrix6<T> &Matrix6<T>::Zero = detail::gMatrix6Zero<T>;
 
  typedef Matrix6<int> Matrix6i;
  typedef Matrix6<double> Matrix6d;
  typedef Matrix6<float> Matrix6f;
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_MATRIX6_HH_