matrix_3d.hpp

#pragma once
 
 
 
#include "_cexports/c_core_3d.h"
 
#include "global.hpp"
 
 
#include "point_3d.hpp"
#include "core_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 // NOLINT
    {
      static_assert(N == 9, "CVB: Matrix3D must have 9 elements");
      std::copy(std::begin(list), std::end(list), matrix_.data()->data());
    }
 
 
    Matrix3D(double alpha, double beta, double gamma)
    {
      Internal::DoResCall([&]()
        {
          auto thisData = reinterpret_cast<CExports::CVC3DMatrix*>(this);
          return CVB_CALL_CAPI(CVC3DRotationMatrixFromRotationAngles(alpha, beta, gamma, *thisData));
        });
    }
 
 
    Matrix3D(double alpha, double beta, double gamma, Factors3D scale)
    {
      Internal::DoResCall([&]()
        {
          auto thisData = reinterpret_cast<CExports::CVC3DMatrix*>(this);
          return CVB_CALL_CAPI(CVC3DMatrixFromRotationAnglesScale(alpha, beta, gamma, *reinterpret_cast<CExports::CVC3DFactors*>(&scale), *thisData));
        });
    }
 
 
    bool TryGetRotationAngles(double& alpha, double& beta, double& gamma) const noexcept
    {
      auto thisData = reinterpret_cast<const CExports::CVC3DMatrix*>(this);
      return CVB_CALL_CAPI(CVC3DRotationAnglesFromRotationMatrix(*thisData, alpha, beta, gamma)) == Cvb::CExports::CVC_ERROR_CODES::CVC_E_OK;
    }
 
 
    void RotationAnglesScale(double& alpha, double& beta, double& gamma, Factors3D &scale, double &precision) const
    {
      Internal::DoResCall([&]()
        {
          precision = -1.0;
          auto thisData = reinterpret_cast<const CExports::CVC3DMatrix*>(this);
          auto errcode = CVB_CALL_CAPI(CVC3DRotationAnglesScaleFromMatrix(*thisData, alpha, beta, gamma, *reinterpret_cast<CExports::CVC3DFactors*>(&scale)));
 
          // if not a valid matrix, then estimate alpha, beta, gamma, scale
          if (errcode< 0)          
            return CVB_CALL_CAPI(CVC3DRotationAnglesScaleFromMatrixApproximation(*thisData, alpha, beta, gamma, *reinterpret_cast<CExports::CVC3DFactors*>(&scale), precision));          
          else          
            return errcode;          
        });
    }
 
 
    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(); // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
    }
 
 
 
    double* operator[](int row) noexcept
    {
      return matrix_[row].data(); // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
    }
 
 
    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));
      });
    }
 
 
    Matrix3D Inverse()
    {
      auto result = *this;
      result.Invert();
      return result;
    }
 
 
    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
    Point3D<double>{{
      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
 
}