gz/math/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 ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_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: Matrix6(const Matrix6<T> &_m) = default;
 
      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> &_mat) = default;
 
      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, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)][
                         clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)];
     }
 
     public: inline T &operator()(const size_t _row, const size_t _col)
     {
       return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]
                        [clamp(_col, IGN_ZERO_SIZE_T, IGN_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;
    }
  }
}
#endif