433 lines
No EOL
13 KiB
C++
433 lines
No EOL
13 KiB
C++
//==================================================================================================
|
|
/*
|
|
ROTGEN - Runtime Overlay for Eigen
|
|
Copyright : CODE RECKONS
|
|
SPDX-License-Identifier: BSL-1.0
|
|
*/
|
|
//==================================================================================================
|
|
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
#include <iostream>
|
|
|
|
namespace rotgen
|
|
{
|
|
template<typename T, int Options = T::storage_order, typename Stride = stride>
|
|
class ref;
|
|
|
|
namespace detail
|
|
{
|
|
template<typename Ref, int Rows, int Cols, bool Inner, bool IsConst>
|
|
struct compute_block_type
|
|
{
|
|
using ref_t = std::conditional_t<IsConst, typename Ref::parent const, typename Ref::parent>;
|
|
|
|
using type = std::conditional_t < storage_status<Rows,Cols,Rows,Cols>
|
|
, Eigen::Block<ref_t, Rows, Cols, Inner>
|
|
, Eigen::Block<ref_t, Eigen::Dynamic, Eigen::Dynamic, Inner>
|
|
>;
|
|
};
|
|
|
|
template<typename T, int O, typename S, int Rows, int Cols, bool Inner, bool IsConst>
|
|
struct compute_block_type<ref<T,O,S>,Rows,Cols,Inner,IsConst>
|
|
: compute_block_type<typename ref<T,O,S>::parent,Rows,Cols,Inner,IsConst>
|
|
{
|
|
};
|
|
|
|
template<typename Ref, int Rows, int Cols, bool Inner, bool IsConst>
|
|
using block_type = typename compute_block_type<Ref,Rows,Cols,Inner,IsConst>::type;
|
|
}
|
|
|
|
template< typename Ref
|
|
, int Rows = Dynamic, int Cols = Dynamic
|
|
, bool Inner = false , int ForceStorageOrder = -1
|
|
>
|
|
class block : private detail::block_type<std::remove_const_t<Ref>, Rows, Cols, Inner,std::is_const_v<Ref> >
|
|
{
|
|
static_assert ( concepts::entity<Ref>
|
|
, "[ROTGEN][CRITICAL] - Block of non-rotgen type instanciated"
|
|
);
|
|
|
|
public:
|
|
using rotgen_tag = void;
|
|
using rotgen_block_tag = void;
|
|
using parent = detail::block_type<std::remove_const_t<Ref>, Rows, Cols, Inner,std::is_const_v<Ref>>;
|
|
using value_type = typename parent::value_type;
|
|
using Index = typename parent::Index;
|
|
|
|
static constexpr int storage_order = (ForceStorageOrder == -1) ? Ref::storage_order : ForceStorageOrder;
|
|
static constexpr bool is_immutable = std::is_const_v<Ref>;
|
|
|
|
static constexpr bool IsRowMajor = parent::IsRowMajor;
|
|
|
|
using concrete_type = matrix<value_type,Rows,Cols,storage_order>;
|
|
using concrete_dynamic_type = matrix<value_type,Dynamic,Dynamic,storage_order>;
|
|
|
|
template<typename ET>
|
|
using as_concrete_type = as_concrete_t<ET, matrix>;
|
|
|
|
static constexpr int Options = Ref::Options;
|
|
static constexpr int RowsAtCompileTime = Rows;
|
|
static constexpr int ColsAtCompileTime = Cols;
|
|
static constexpr bool IsVectorAtCompileTime = Ref::IsVectorAtCompileTime;
|
|
static constexpr bool is_defined_static = Rows!=-1 && Cols!=-1;
|
|
static constexpr bool has_static_storage = storage_status<Rows,Cols,Rows,Cols>;
|
|
|
|
public:
|
|
block(const block& other) = default;
|
|
block(block&& other) = default;
|
|
block& operator=(const block&) = default;
|
|
block& operator=(block&&) = default;
|
|
|
|
block(Ref const& r, std::size_t i0, std::size_t j0, std::size_t ni, std::size_t nj)
|
|
requires(is_immutable)
|
|
: parent(r.base(),i0,j0,ni,nj)
|
|
{}
|
|
|
|
block(Ref const& r, std::size_t i0, std::size_t j0)
|
|
requires(Rows != -1 && Cols != -1 && is_immutable)
|
|
: parent(r.base(),i0,j0,Rows,Cols)
|
|
{}
|
|
|
|
block(Ref& r, std::size_t i0, std::size_t j0, std::size_t ni, std::size_t nj)
|
|
requires(!is_immutable)
|
|
: parent(r.base(),i0,j0,ni,nj)
|
|
{}
|
|
|
|
block(Ref& r, std::size_t i0, std::size_t j0)
|
|
requires(Rows != -1 && Cols != -1 && !is_immutable)
|
|
: parent(r.base(),i0,j0,Rows,Cols)
|
|
{}
|
|
|
|
template<typename B, Index R, Index C, bool I, int FS>
|
|
block(block<B,R,C,I,FS> const& other) : parent(other.base())
|
|
{}
|
|
|
|
template<typename OtherDerived>
|
|
block(const Eigen::MatrixBase<OtherDerived>& other) : parent(other) {}
|
|
|
|
template<typename OtherDerived>
|
|
block(const Eigen::EigenBase<OtherDerived>& other) : parent(other) {}
|
|
|
|
block(concepts::entity auto const& other) : parent(other.base())
|
|
{}
|
|
|
|
block& operator=(concepts::entity auto const& other)
|
|
{
|
|
parent::operator=(other.base());
|
|
return *this;
|
|
}
|
|
|
|
template<typename OtherDerived>
|
|
block& operator=(const Eigen::MatrixBase<OtherDerived>& other)
|
|
{
|
|
parent::operator=(other);
|
|
return *this;
|
|
}
|
|
|
|
template<typename OtherDerived>
|
|
block& operator=(const Eigen::EigenBase<OtherDerived>& other)
|
|
{
|
|
parent::operator=(other);
|
|
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 return as_concrete_type<decltype(base().normalized())>(base().normalized());
|
|
}
|
|
|
|
auto transpose() const
|
|
{
|
|
if constexpr(use_expression_templates) return base().transpose();
|
|
else return as_concrete_type<decltype(base().transpose())>(base().transpose());
|
|
}
|
|
|
|
auto adjoint() const
|
|
{
|
|
if constexpr(use_expression_templates) return base().adjoint();
|
|
else return as_concrete_type<decltype(base().adjoint())>(base().adjoint());
|
|
}
|
|
|
|
auto conjugate() const
|
|
{
|
|
if constexpr(use_expression_templates) return base().conjugate();
|
|
else return as_concrete_type<decltype(base().conjugate())>(base().conjugate());
|
|
}
|
|
|
|
void normalize() requires(!is_immutable && IsVectorAtCompileTime)
|
|
{
|
|
parent::normalize();
|
|
}
|
|
|
|
void transposeInPlace() requires(!is_immutable) { parent::transposeInPlace(); }
|
|
void adjointInPlace() requires(!is_immutable) { parent::adjointInPlace(); }
|
|
|
|
auto cwiseAbs() const
|
|
{
|
|
if constexpr(!use_expression_templates) return concrete_type{parent::cwiseAbs()};
|
|
else return base().cwiseAbs();
|
|
}
|
|
|
|
auto cwiseAbs2() const
|
|
{
|
|
if constexpr(!use_expression_templates) return concrete_type{parent::cwiseAbs2()};
|
|
else return base().cwiseAbs2();
|
|
}
|
|
|
|
auto cwiseInverse() const
|
|
{
|
|
if constexpr(!use_expression_templates) return concrete_type{parent::cwiseInverse()};
|
|
else return base().cwiseInverse();
|
|
}
|
|
|
|
auto cwiseSqrt() const
|
|
{
|
|
if constexpr(!use_expression_templates) return concrete_type{parent::cwiseSqrt()};
|
|
else return base().cwiseSqrt();
|
|
}
|
|
|
|
static concrete_type Constant(value_type value) requires (Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Constant(Rows, Cols, static_cast<value_type>(value));
|
|
}
|
|
|
|
static concrete_type Constant(int rows, int cols, value_type 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<value_type>(value));
|
|
}
|
|
|
|
static concrete_type Identity() requires (Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Identity(Rows, Cols);
|
|
}
|
|
|
|
static concrete_type 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 concrete_type Zero() requires (Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Zero(Rows, Cols);
|
|
}
|
|
|
|
static concrete_type 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 concrete_type Ones() requires (Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Ones(Rows, Cols);
|
|
}
|
|
|
|
static concrete_type 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 concrete_type Random() requires (Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Random(Rows, Cols);
|
|
}
|
|
|
|
static concrete_type 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);
|
|
}
|
|
|
|
block& setOnes() requires(!is_immutable)
|
|
{
|
|
*this = parent::Ones(rows(), cols());
|
|
return *this;
|
|
}
|
|
|
|
block& setOnes(int r, int c) requires(!is_immutable)
|
|
{
|
|
*this = parent::Ones(r, c);
|
|
return *this;
|
|
}
|
|
|
|
block& setZero() requires(!is_immutable)
|
|
{
|
|
*this = parent::Zero(rows(), cols());
|
|
return *this;
|
|
}
|
|
|
|
block& setZero(int r, int c) requires(!is_immutable)
|
|
{
|
|
*this = parent::Zero(r,c);
|
|
return *this;
|
|
}
|
|
|
|
block& setConstant(value_type value) requires(!is_immutable)
|
|
{
|
|
*this = parent::Constant(rows(), cols(), value);
|
|
return *this;
|
|
}
|
|
|
|
block& setConstant(int r, int c, value_type value) requires(!is_immutable)
|
|
{
|
|
*this = parent::Constant(r,c, value);
|
|
return *this;
|
|
}
|
|
|
|
block& setRandom() requires(!is_immutable)
|
|
{
|
|
*this = parent::Random(rows(),cols());
|
|
return *this;
|
|
}
|
|
|
|
block& setRandom(int r, int c) requires(!is_immutable)
|
|
{
|
|
*this = parent::Random(r,c);
|
|
return *this;
|
|
}
|
|
|
|
block& setIdentity() requires(!is_immutable)
|
|
{
|
|
*this = parent::Identity(rows(),cols());
|
|
return *this;
|
|
}
|
|
|
|
block& setIdentity(int r, int c) requires(!is_immutable)
|
|
{
|
|
*this = parent::Identity(r,c);
|
|
return *this;
|
|
}
|
|
|
|
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, Index j) requires(!is_immutable)
|
|
{
|
|
return base()(i,j);
|
|
}
|
|
|
|
value_type& operator()(Index i) requires(!is_immutable && IsVectorAtCompileTime)
|
|
{
|
|
return base().data()[i];
|
|
}
|
|
|
|
value_type& operator[](Index i) requires(!is_immutable && IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
value_type operator()(Index i, Index j) const { return base()(i,j); }
|
|
value_type operator()(Index i) const requires(IsVectorAtCompileTime) { return base().data()[i]; }
|
|
value_type operator[](Index i) const requires(IsVectorAtCompileTime) { return (*this)(i); }
|
|
|
|
using parent::innerStride;
|
|
using parent::outerStride;
|
|
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;
|
|
}
|
|
|
|
Index startRow() const { return base().startRow(); }
|
|
Index startCol() const { return base().startCol(); }
|
|
|
|
block& operator+=(block const& rhs) requires(!is_immutable)
|
|
{
|
|
static_cast<parent&>(*this) += static_cast<parent const&>(rhs);
|
|
return *this;
|
|
}
|
|
|
|
block& operator-=(block const& rhs) requires(!is_immutable)
|
|
{
|
|
static_cast<parent&>(*this) -= static_cast<parent const&>(rhs);
|
|
return *this;
|
|
}
|
|
|
|
concrete_type operator-() const
|
|
{
|
|
return concrete_type(static_cast<parent const&>(*this).operator-());
|
|
}
|
|
|
|
block& operator*=(block const& rhs) requires(!is_immutable)
|
|
{
|
|
static_cast<parent&>(*this) *= static_cast<parent const&>(rhs);
|
|
return *this;
|
|
}
|
|
|
|
block& operator*=(value_type rhs) requires(!is_immutable)
|
|
{
|
|
static_cast<parent&>(*this) *= rhs;
|
|
return *this;
|
|
}
|
|
|
|
block& operator/=(value_type rhs) requires(!is_immutable)
|
|
{
|
|
static_cast<parent&>(*this) /= rhs;
|
|
return *this;
|
|
}
|
|
};
|
|
} |