rotgen/include/rotgen/fixed/matrix.hpp

//==================================================================================================
/*
  ROTGEN - Runtime Overlay for Eigen
  Copyright : CODE RECKONS
  SPDX-License-Identifier: BSL-1.0
*/
//==================================================================================================
#pragma once

#include <Eigen/Dense>
#include <iostream>

namespace rotgen
{
  namespace detail
  {
    template<typename Scalar, int Rows, int Cols, int Opts, int MaxRows, int MaxCols>
    using storage_type = std::conditional_t < storage_status<Rows,Cols,MaxRows,MaxCols>
                                            , Eigen::Matrix<Scalar, Rows, Cols, Opts, MaxRows, MaxCols>
                                            , Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Opts>
                                            >;
  }

  template< typename Scalar
          , int Rows = Dynamic , int Cols = Dynamic
          , int Opts = detail::force_order<Rows,Cols>
          , int MaxRows = Rows, int MaxCols = Cols
          >
  class matrix : private detail::storage_type<Scalar, Rows, Cols, Opts, MaxRows, MaxCols>
  {
    public:
    using rotgen_tag  = void;
    using parent      = detail::storage_type<Scalar, Rows, Cols, Opts, MaxRows, MaxCols>;
    using value_type  = Scalar;
    using Index       = typename parent::Index;

    static constexpr auto storage_order = Opts & 1;
    using concrete_type = matrix;
    using concrete_dynamic_type = matrix<value_type>;

    static constexpr auto   Flags                 = parent::Flags;
    static constexpr int    RowsAtCompileTime     = Rows;
    static constexpr int    ColsAtCompileTime     = Cols;
    static constexpr bool   IsVectorAtCompileTime = (RowsAtCompileTime == 1) || (ColsAtCompileTime == 1);
    static constexpr int    Options               = parent::Options;
    static constexpr bool   IsRowMajor            = parent::IsRowMajor;

    template<typename ET>
    using as_concrete_type = as_concrete_t<ET, matrix>;

    static constexpr bool is_defined_static   = Rows!=-1 && Cols!=-1;
    static constexpr bool has_static_storage  = storage_status<Rows,Cols,MaxRows,MaxCols>;

    public:

    matrix() requires(has_static_storage) {}
    matrix() requires(!has_static_storage) : parent(Rows > 0 ? Rows : 0, Cols > 0 ? Cols : 0){}
    matrix(Index r, Index c) : parent(r, c) {}

    matrix(const matrix& other)       = default;
    matrix(matrix&& other)            = default;
    matrix& operator=(const matrix&)  = default;
    matrix& operator=(matrix&&)       = default;

    matrix(std::initializer_list<std::initializer_list<Scalar>> init) : parent(init)
    {}

    matrix(Index n) requires(IsVectorAtCompileTime && (Rows != 1 || Cols != 1))
    : parent(n)
    {}

    matrix(Scalar v) requires(Rows == 1  && Cols == 1) : parent
    ( [&]()
      {
        if constexpr(has_static_storage) return parent(v);
        else return parent{1,1};
      }()
    )
    {
      if constexpr(!has_static_storage) (*this)(0) = v;
    }

    explicit matrix(std::initializer_list<Scalar> init)
    requires(IsVectorAtCompileTime)
    : parent( [&]()
              {
                if constexpr(has_static_storage) return parent{};
                else return parent{Rows,Cols};
              }()
            )
    {
      auto first = init.begin();
      for(rotgen::Index i=0; i < parent::size(); i++)
        (*this)(i) = first[i];
    }

    matrix(concepts::entity auto const& other) : parent(other.base())
    {}

    template<typename OtherDerived>
    matrix(const Eigen::MatrixBase<OtherDerived>& other) : parent(other)
    {}

    template<typename OtherDerived>
    matrix(const Eigen::EigenBase<OtherDerived>& other) : parent(other)
    {}

    template<typename OtherDerived>
    matrix& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
      parent::operator=(other);
      return *this;
    }

    template<typename OtherDerived>
    matrix& operator=(const Eigen::EigenBase<OtherDerived>& other)
    {
      parent::operator=(other);
      return *this;
    }

    matrix& operator=(concepts::entity auto const& other)
    {
      parent::operator=(other.base());
      return *this;
    }

    parent&       base()         { return static_cast<parent&>(*this);        }
    parent const& base() const   { return static_cast<parent const&>(*this);  }

    auto evaluate() const
    {
      auto res = base().eval();
      return as_concrete_type<decltype(res)>(res);
    }

    decltype(auto) noalias() const
    {
      if constexpr(use_expression_templates)  return base().noalias();
      else                                    return *this;
    }

    decltype(auto) noalias()
    {
      if constexpr(use_expression_templates)  return base().noalias();
      else                                    return *this;
    }

    auto normalized() const requires(IsVectorAtCompileTime)
    {
      if constexpr(use_expression_templates) return base().normalized();
      else
      {
        auto res = base().normalized();
        return as_concrete_type<decltype(res)>(res);
      }
    }

    auto transpose() const
    {
      if constexpr(use_expression_templates) return base().transpose();
      else
      {
        auto res = base().transpose();
        return as_concrete_type<decltype(res)>(res);
      }
    }

    auto conjugate() const
    {
      auto res = base().conjugate();
      return as_concrete_type<decltype(res)>(res);
    }

    auto adjoint() const
    {
      auto res = base().adjoint();
      return as_concrete_type<decltype(res)>(res);
    }

    void normalize()        requires(IsVectorAtCompileTime)
    {
      parent::normalize();
    }

    void transposeInPlace() { parent::transposeInPlace(); }
    void adjointInPlace()   { parent::adjointInPlace();   }

    auto cwiseAbs() const
    {
      if constexpr(!use_expression_templates) return matrix{parent::cwiseAbs()};
      else                                    return base().cwiseAbs();
    }

    auto cwiseAbs2() const
    {
      if constexpr(!use_expression_templates) return matrix{parent::cwiseAbs2()};
      else                                    return base().cwiseAbs2();
    }

    auto cwiseInverse() const
    {
      if constexpr(!use_expression_templates) return matrix{parent::cwiseInverse()};
      else                                    return base().cwiseInverse();
    }

    auto cwiseSqrt() const
    {
      if constexpr(!use_expression_templates) return matrix{parent::cwiseSqrt()};
      else                                    return base().cwiseSqrt();
    }

    void resize(int new_size) requires(IsVectorAtCompileTime && (Rows == -1 || Cols == -1))
    {
      parent::resize(new_size);
    }

    void resize(int new_rows, int new_cols) requires(Rows == -1 && Cols == -1)
    {
      parent::resize(new_rows, new_cols);
    }

    void conservativeResize(int new_size) requires(IsVectorAtCompileTime && (Rows == -1 || Cols == -1))
    {
      parent::conservativeResize(new_size);
    }

    void conservativeResize(int new_rows, int new_cols) requires(Rows == -1 && Cols == -1)
    {
      parent::conservativeResize(new_rows, new_cols);
    }

    static matrix Constant(Scalar value) requires (Rows != -1 && Cols != -1)
    {
      return parent::Constant(Rows, Cols, static_cast<Scalar>(value));
    }

    static matrix Constant(int rows, int cols, Scalar value)
    {
      if constexpr(Rows != -1) assert(rows == Rows &&  "Mismatched between dynamic and static row size");
      if constexpr(Cols != -1) assert(cols == Cols &&  "Mismatched between dynamic and static column size");
      return parent::Constant(rows, cols, static_cast<Scalar>(value));
    }

    static matrix Identity() requires (Rows != -1 && Cols != -1)
    {
      return parent::Identity(Rows, Cols);
    }

    static matrix Identity(int rows, int cols)
    {
      if constexpr(Rows != -1) assert(rows == Rows &&  "Mismatched between dynamic and static row size");
      if constexpr(Cols != -1) assert(cols == Cols &&  "Mismatched between dynamic and static column size");
      return parent::Identity(rows, cols);
    }

    static matrix Ones() requires (Rows != -1 && Cols != -1)
    {
      return parent::Ones(Rows, Cols);
    }

    static matrix Ones(int rows, int cols)
    {
      if constexpr(Rows != -1) assert(rows == Rows &&  "Mismatched between dynamic and static row size");
      if constexpr(Cols != -1) assert(cols == Cols &&  "Mismatched between dynamic and static column size");
      return parent::Ones(rows, cols);
    }

    static matrix Zero() requires (Rows != -1 && Cols != -1)
    {
      return parent::Zero(Rows, Cols);
    }

    static matrix Zero(int rows, int cols)
    {
      if constexpr(Rows != -1) assert(rows == Rows &&  "Mismatched between dynamic and static row size");
      if constexpr(Cols != -1) assert(cols == Cols &&  "Mismatched between dynamic and static column size");
      return parent::Zero(rows, cols);
    }

    static matrix Random() requires (Rows != -1 && Cols != -1)
    {
      return parent::Random(Rows, Cols);
    }

    static matrix Random(int rows, int cols)
    {
      if constexpr(Rows != -1) assert(rows == Rows &&  "Mismatched between dynamic and static row size");
      if constexpr(Cols != -1) assert(cols == Cols &&  "Mismatched between dynamic and static column size");
      return parent::Random(rows, cols);
    }

    template<int P>
    value_type lpNorm() const
    {
      static_assert(P == 1 || P == 2 || P == Infinity);
      return parent::template lpNorm<P>();
    }

    value_type& operator[](Index i) requires(IsVectorAtCompileTime)
    {
      return (*this)(i);
    }

    value_type operator[](Index i) const requires(IsVectorAtCompileTime)
    {
      return (*this)(i);
    }

    using parent::operator();
    using parent::rows;
    using parent::cols;
    using parent::size;
    using parent::prod;
    using parent::mean;
    using parent::trace;
    using parent::squaredNorm;
    using parent::norm;
    using parent::sum;
    using parent::data;


    auto minCoeff() const { return parent::minCoeff(); }
    auto maxCoeff() const { return parent::maxCoeff(); }

    template<std::integral IndexType>
    auto minCoeff(IndexType* row, IndexType* col) const
    {
      Index r,c;
      auto result = parent::minCoeff(&r, &c);
      *row = r;
      *col = c;
      return result;
    }

    template<std::integral IndexType>
    auto maxCoeff(IndexType* row, IndexType* col) const
    {
      Index r,c;
      auto result = parent::maxCoeff(&r, &c);
      *row = r;
      *col = c;
      return result;
    }

    matrix& setOnes()
    {
      *this = parent::Ones(rows(),cols());
      return *this;
    }

    matrix& setOnes(int r, int c)
    {
      *this = parent::Ones(r, c);
      return *this;
    }

    matrix& setZero()
    {
      *this = parent::Zero(rows(),cols());
      return *this;
    }

    matrix& setZero(int r, int c)
    {
      *this = parent::Zero(r, c);
      return *this;
    }

    matrix& setConstant(Scalar value)
    {
      *this = parent::Constant(rows(),cols(), static_cast<Scalar>(value));
      return *this;
    }

    matrix& setConstant(int r, int c, Scalar value)
    {
      *this = parent::Constant(r, c, static_cast<Scalar>(value));
      return *this;
    }

    matrix& setRandom()
    {
      *this = parent::Random(rows(),cols());
      return *this;
    }

    matrix& setRandom(int r, int c)
    {
      *this = parent::Random(r, c);
      return *this;
    }

    matrix& setIdentity()
    {
      *this = parent::Identity(rows(),cols());
      return *this;
    }

    matrix& setIdentity(int r, int c)
    {
      *this = parent::Identity(r, c);
      return *this;
    }

    matrix& operator+=(matrix const& rhs)
    {
      static_cast<parent&>(*this) += rhs.base();
      return *this;
    }

    matrix& operator-=(matrix const& rhs)
    {
      static_cast<parent&>(*this) -= rhs.base();
      return *this;
    }

    matrix operator-() const
    {
      return matrix(base()(*this).operator-());
    }

    matrix& operator*=(matrix const& rhs)
    {
      static_cast<parent&>(*this) *= rhs.base();
      return *this;
    }

    matrix& operator*=(Scalar rhs)
    {
      static_cast<parent&>(*this) *= rhs;
      return *this;
    }

    matrix& operator/=(Scalar rhs)
    {
      static_cast<parent&>(*this) /= rhs;
      return *this;
    }
  };
}