matrix_3d.hpp

#pragma once



#include "_cexports/c_core_3d.h"

#include "global.hpp"


#include "point_3d.hpp"

namespace Cvb
{

CVB_BEGIN_INLINE_NS


class Matrix3D;



inline Matrix3D operator+(const Matrix3D & lhs, const Matrix3D & rhs);


inline Matrix3D operator-(const Matrix3D & lhs, const Matrix3D & rhs);



inline Matrix3D operator*(const Matrix3D & lhs, const Matrix3D & rhs);


class Matrix3D final
{
  public:


    static Matrix3D Identity() noexcept
    {
      return  {{
                1.0, 0.0, 0.0,
                0.0, 1.0, 0.0,
                0.0, 0.0, 1.0
              }};
    }


    Matrix3D() noexcept = default;


    template<std::size_t N>
    Matrix3D(const double(&list)[N]) noexcept
    {
      static_assert(N == 9, "CVB: Matrix3D must have 9 elements");
      std::copy(std::begin(list), std::end(list), matrix_.data()->data());
    }



    Matrix3D(double roll, double pitch, double yaw)
    {
      Internal::DoResCall([&]()
      {
        auto thisData = reinterpret_cast<CExports::CVC3DMatrix*>(this);
        return CVB_CALL_CAPI(CVC3DRotationMatrixFromRollPitchYaw(roll, pitch, yaw, *thisData));
      });

    }


    void RollPitchYaw(double & roll, double & pitch, double & yaw) const
    {
      Internal::DoResCall([&]()
      {
        auto thisData = reinterpret_cast<const CExports::CVC3DMatrix*>(this);
        return CVB_CALL_CAPI(CVC3DRollPitchYawFromRotationMatrix(*thisData, roll, pitch, yaw));
      });

    }


    const double* operator[](int row) const noexcept
    {
      return matrix_[row].data();
    }



    double* operator[](int row) noexcept
    {
      return matrix_[row].data();
    }


    const double& At(int row, int column) const noexcept
    {
      return (*this)[row][column];
    }


    double& At(int row, int column) noexcept
    {
      return (*this)[row][column];
    }


    double Det() const
    {
      return Internal::DoResCallValueOut<double>([&](double & value)
      {
        auto thisData = reinterpret_cast<const CExports::CVC3DMatrix*>(this);
        return CVB_CALL_CAPI(CVC3DMatrixDeterminant(*thisData, value));
      });
    }



    void Invert()
    {
      Internal::DoResCall([&]()
      {
        auto data = reinterpret_cast<const CExports::CVC3DMatrix*>(this);
        auto dataInv = reinterpret_cast<CExports::CVC3DMatrix*>(this);
        return CVB_CALL_CAPI(CVC3DInvertMatrix(*data, *dataInv));
      });
    }


    bool operator==(const Matrix3D& matrix) const noexcept
    {
      return matrix_ == matrix.matrix_;
    }


    bool operator!=(const Matrix3D& matrix) const noexcept
    {
      return !(*this == matrix);
    }


    Matrix3D& operator +=(const Matrix3D & matrix) noexcept
    {
      *this = *this + matrix;
      return *this;
    }


    Matrix3D& operator -=(const Matrix3D & matrix) noexcept
    {
      *this = *this - matrix;
      return *this;
    }


    Matrix3D& operator *=(const Matrix3D & matrix)
    {
      *this = *this * matrix;
      return *this;
    }


    Matrix3D& operator *=(const double & value) noexcept
    {
      for (auto & line : matrix_)
        for (auto & element : line)
          element *= value;
      return *this;
    }


    Matrix3D& operator /=(const double & value) noexcept
    {
      for(auto & line : matrix_)
        for(auto & element : line)
          element /= value;
      return *this;
    }



  private:

    
    std::array<std::array<double, 3>, 3> matrix_{};
};




inline Matrix3D operator+(const Matrix3D & lhs, const Matrix3D & rhs)
{
  return
  {{
      lhs[0][0] + rhs[0][0], lhs[0][1] + rhs[0][1], lhs[0][2] + rhs[0][2],
      lhs[1][0] + rhs[1][0], lhs[1][1] + rhs[1][1], lhs[1][2] + rhs[1][2],
      lhs[2][0] + rhs[2][0], lhs[2][1] + rhs[2][1], lhs[2][2] + rhs[2][2],
  }};
}


inline Matrix3D operator-(const Matrix3D & lhs, const Matrix3D & rhs)
{
  return
  { {
      lhs[0][0] - rhs[0][0], lhs[0][1] - rhs[0][1], lhs[0][2] - rhs[0][2],
      lhs[1][0] - rhs[1][0], lhs[1][1] - rhs[1][1], lhs[1][2] - rhs[1][2],
      lhs[2][0] - rhs[2][0], lhs[2][1] - rhs[2][1], lhs[2][2] - rhs[2][2],
    } };
}


inline Matrix3D operator*(const Matrix3D & lhs, const Matrix3D & rhs)
{

  return Internal::DoResCallValueOut<Matrix3D>([&](Matrix3D & value)
  {
    auto valueData = reinterpret_cast<CExports::CVC3DMatrix*>(&value);
    auto lhsData = reinterpret_cast<const CExports::CVC3DMatrix*>(&lhs);
    auto rhsData = reinterpret_cast<const CExports::CVC3DMatrix*>(&rhs);
    return CVB_CALL_CAPI(CVC3DMultiplyMatrices(*lhsData,
      *rhsData,
      *valueData));
  });

}


inline Point3D<double> operator*(const Matrix3D & lhs, const Point3D<double> & rhs)
{
  return
  {{
      lhs[0][0] * rhs[0] + lhs[0][1] * rhs[1] + lhs[0][2] * rhs[2],
      lhs[1][0] * rhs[0] + lhs[1][1] * rhs[1] + lhs[1][2] * rhs[2],
      lhs[2][0] * rhs[0] + lhs[2][1] * rhs[1] + lhs[2][2] * rhs[2],
  }};
}


inline Matrix3D operator*(const Matrix3D & lhs, const double & rhs)
{
  return
  {{
      lhs[0][0] * rhs, lhs[0][1] * rhs, lhs[0][2] * rhs,
      lhs[1][0] * rhs, lhs[1][1] * rhs, lhs[1][2] * rhs,
      lhs[2][0] * rhs, lhs[2][1] * rhs, lhs[2][2] * rhs
  }};
}


inline Matrix3D operator*(const double & lhs, const Matrix3D & rhs)
{
  return rhs * lhs;
}


inline Matrix3D operator/(const Matrix3D & lhs, const double & rhs)
{
  return
  {{
      lhs[0][0] / rhs, lhs[0][1] / rhs, lhs[0][2] / rhs,
      lhs[1][0] / rhs, lhs[1][1] / rhs, lhs[1][2] / rhs,
      lhs[2][0] / rhs, lhs[2][1] / rhs, lhs[2][2] / rhs
  }};
}






CVB_END_INLINE_NS

}