535 lines
15 KiB
C++
535 lines
15 KiB
C++
//==================================================================================================
|
|
/*
|
|
ROTGEN - Runtime Overlay for Eigen
|
|
Copyright : CODE RECKONS
|
|
SPDX-License-Identifier: BSL-1.0
|
|
*/
|
|
//==================================================================================================
|
|
#pragma once
|
|
|
|
#include <rotgen/detail/assert.hpp>
|
|
|
|
#include <rotgen/concepts.hpp>
|
|
#include <rotgen/container/block/dynamic/impl.hpp>
|
|
#include <rotgen/container/matrix/dynamic.hpp>
|
|
|
|
#include <initializer_list>
|
|
|
|
namespace rotgen
|
|
{
|
|
template<typename T,
|
|
int Options = 0,
|
|
typename Stride = std::conditional_t<T::IsVectorAtCompileTime,
|
|
inner_stride<1>,
|
|
outer_stride<>>>
|
|
class ref;
|
|
|
|
template<typename Ref,
|
|
int Rows = Dynamic,
|
|
int Cols = Dynamic,
|
|
bool Inner = false>
|
|
class block : public find_block<Ref, Rows, Cols>
|
|
{
|
|
public:
|
|
static_assert(concepts::entity<Ref>,
|
|
"[ROTGEN][CRITICAL] - Block of non-rotgen type instanciated");
|
|
|
|
using parent = find_block<Ref, Rows, Cols>;
|
|
using rotgen_tag = void;
|
|
using rotgen_block_tag = void;
|
|
using value_type = typename std::remove_const_t<Ref>::value_type;
|
|
|
|
static constexpr int RowsAtCompileTime = Rows;
|
|
static constexpr int ColsAtCompileTime = Cols;
|
|
static constexpr int MaxRowsAtCompileTime =
|
|
(Rows != Dynamic) ? Rows : Ref::MaxRowsAtCompileTime;
|
|
static constexpr int MaxColsAtCompileTime =
|
|
(Cols != Dynamic) ? Cols : Ref::MaxColsAtCompileTime;
|
|
static constexpr bool IsRowMajor =
|
|
(MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? true
|
|
: (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1)
|
|
? false
|
|
: Ref::IsRowMajor;
|
|
|
|
static constexpr int storage_order = IsRowMajor ? RowMajor : ColMajor;
|
|
|
|
static constexpr bool has_same_storage_order =
|
|
storage_order == Ref::storage_order;
|
|
static constexpr int InnerStrideAtCompileTime =
|
|
has_same_storage_order ? Ref::InnerStrideAtCompileTime
|
|
: Ref::OuterStrideAtCompileTime;
|
|
static constexpr int OuterStrideAtCompileTime =
|
|
has_same_storage_order ? Ref::OuterStrideAtCompileTime
|
|
: Ref::InnerStrideAtCompileTime;
|
|
|
|
static constexpr bool is_immutable = std::is_const_v<Ref>;
|
|
using concrete_type = matrix<value_type, Rows, Cols, storage_order>;
|
|
using transposed_type = matrix<value_type, Cols, Rows, storage_order>;
|
|
using concrete_dynamic_type =
|
|
matrix<value_type, Dynamic, Dynamic, storage_order>;
|
|
|
|
static constexpr bool IsVectorAtCompileTime =
|
|
(RowsAtCompileTime == 1) || (ColsAtCompileTime == 1);
|
|
static constexpr int Options = IsRowMajor ? RowMajor : ColMajor;
|
|
|
|
static constexpr bool is_defined_static = Rows != -1 && Cols != -1;
|
|
static constexpr bool has_static_storage = false;
|
|
|
|
using parent::operator=;
|
|
|
|
template<concepts::entity Src>
|
|
block& operator=(Src const& other)
|
|
requires(!is_immutable)
|
|
{
|
|
if constexpr (IsVectorAtCompileTime && Src::IsVectorAtCompileTime)
|
|
{
|
|
ROTGEN_ASSERT(parent::size() == other.size(),
|
|
"Block assignment from 1D source doesn't match size");
|
|
for (rotgen::Index i = 0; i < parent::size(); ++i)
|
|
(*this)[i] = other[i];
|
|
}
|
|
else if constexpr (IsVectorAtCompileTime && !Src::IsVectorAtCompileTime)
|
|
{
|
|
auto r = other.rows();
|
|
auto c = other.cols();
|
|
|
|
ROTGEN_ASSERT((r == 1 || c == 1), "Block assignment from dynamic sized "
|
|
"source doesn't match static size");
|
|
|
|
for (rotgen::Index i = 0; i < parent::size(); ++i)
|
|
(*this)[i] = other(r == 1 ? 0 : i, c == 1 ? 0 : i);
|
|
}
|
|
else
|
|
{
|
|
ROTGEN_ASSERT(parent::rows() == other.rows() &&
|
|
parent::cols() == other.cols(),
|
|
"Block assignment size mismatch");
|
|
for (rotgen::Index r = 0; r < parent::rows(); ++r)
|
|
for (rotgen::Index c = 0; c < parent::cols(); ++c)
|
|
(*this)(r, c) = other(r, c);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
block(Ref const& r, Index i0, Index j0, Index ni, Index nj)
|
|
requires(!requires { typename Ref::rotgen_block_tag; } && is_immutable)
|
|
: parent(r.base(), i0, j0, ni, nj)
|
|
{
|
|
}
|
|
|
|
block(Ref const& r, Index i0, Index j0, Index ni, Index nj)
|
|
requires(requires { typename Ref::rotgen_block_tag; } && is_immutable)
|
|
: parent(r.base(), i0, j0, ni, nj)
|
|
{
|
|
}
|
|
|
|
block(Ref const& r, Index i0, Index j0)
|
|
requires(!requires { typename Ref::rotgen_block_tag; } && Rows != -1 &&
|
|
Cols != -1 && is_immutable)
|
|
: parent(r.base(), i0, j0, Rows, Cols)
|
|
{
|
|
}
|
|
|
|
block(Ref const& r, Index i0, Index j0)
|
|
requires(requires { typename Ref::rotgen_block_tag; } && Rows != -1 &&
|
|
Cols != -1 && is_immutable)
|
|
: parent(r.base(), i0, j0, Rows, Cols)
|
|
{
|
|
}
|
|
|
|
block(Ref& r, Index i0, Index j0, Index ni, Index nj)
|
|
requires(!requires { typename Ref::rotgen_block_tag; } && !is_immutable)
|
|
: parent(r.base(), i0, j0, ni, nj)
|
|
{
|
|
}
|
|
|
|
block(Ref& r, Index i0, Index j0, Index ni, Index nj)
|
|
requires(requires { typename Ref::rotgen_block_tag; } && !is_immutable)
|
|
: parent(r.base(), i0, j0, ni, nj)
|
|
{
|
|
}
|
|
|
|
block(Ref& r, Index i0, Index j0)
|
|
requires(!requires { typename Ref::rotgen_block_tag; } && Rows != -1 &&
|
|
Cols != -1 && !is_immutable)
|
|
: parent(r.base(), i0, j0, Rows, Cols)
|
|
{
|
|
}
|
|
|
|
block(Ref& r, Index i0, Index j0)
|
|
requires(requires { typename Ref::rotgen_block_tag; } && Rows != -1 &&
|
|
Cols != -1 && !is_immutable)
|
|
: parent(r.base(), i0, j0, Rows, Cols)
|
|
{
|
|
}
|
|
|
|
block(Ref&& r, Index i0, Index j0, Index ni, Index nj)
|
|
requires(!requires { typename Ref::rotgen_block_tag; } && !is_immutable)
|
|
: parent(r.base(), i0, j0, ni, nj)
|
|
{
|
|
}
|
|
|
|
block(Ref&& r, Index i0, Index j0, Index ni, Index nj)
|
|
requires(requires { typename Ref::rotgen_block_tag; } && !is_immutable)
|
|
: parent(r.base(), i0, j0, ni, nj)
|
|
{
|
|
}
|
|
|
|
block(Ref&& r, Index i0, Index j0)
|
|
requires(!requires { typename Ref::rotgen_block_tag; } && Rows != -1 &&
|
|
Cols != -1 && !is_immutable)
|
|
: parent(r.base(), i0, j0, Rows, Cols)
|
|
{
|
|
}
|
|
|
|
block(Ref&& r, Index i0, Index j0)
|
|
requires(requires { typename Ref::rotgen_block_tag; } && Rows != -1 &&
|
|
Cols != -1 && !is_immutable)
|
|
: parent(r.base(), i0, j0, Rows, Cols)
|
|
{
|
|
}
|
|
|
|
block(parent const& base) : parent(base) {}
|
|
|
|
value_type& operator()(Index i, Index j)
|
|
requires(!is_immutable)
|
|
{
|
|
return parent::operator()(i, j);
|
|
}
|
|
|
|
value_type& operator()(Index i)
|
|
requires(!is_immutable && IsVectorAtCompileTime)
|
|
{
|
|
return parent::operator()(i);
|
|
}
|
|
|
|
value_type& operator[](Index i)
|
|
requires(!is_immutable && IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
value_type operator()(Index i, Index j) const
|
|
{
|
|
return parent::operator()(i, j);
|
|
}
|
|
|
|
value_type operator()(Index i) const
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return parent::operator()(i);
|
|
}
|
|
|
|
value_type operator[](Index i) const
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
concrete_type evaluate() const { return concrete_type{*this}; }
|
|
|
|
decltype(auto) noalias() const { return *this; }
|
|
|
|
decltype(auto) noalias() { return *this; }
|
|
|
|
concrete_type normalized() const
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return concrete_type(base().normalized());
|
|
}
|
|
|
|
transposed_type transpose() const
|
|
{
|
|
return transposed_type(base().transpose());
|
|
}
|
|
|
|
concrete_type conjugate() const
|
|
{
|
|
return concrete_type(base().conjugate());
|
|
}
|
|
|
|
transposed_type adjoint() const
|
|
{
|
|
return transposed_type(base().adjoint());
|
|
}
|
|
|
|
concrete_type cwiseAbs() const { return concrete_type(base().cwiseAbs()); }
|
|
|
|
concrete_type cwiseAbs2() const
|
|
{
|
|
return concrete_type(base().cwiseAbs2());
|
|
}
|
|
|
|
concrete_type cwiseInverse() const
|
|
{
|
|
return concrete_type(base().cwiseInverse());
|
|
}
|
|
|
|
concrete_type cwiseSqrt() const
|
|
{
|
|
return concrete_type(base().cwiseSqrt());
|
|
}
|
|
|
|
void normalize()
|
|
requires(!is_immutable && IsVectorAtCompileTime)
|
|
{
|
|
parent::normalize();
|
|
}
|
|
|
|
void transposeInPlace()
|
|
requires(!is_immutable)
|
|
{
|
|
parent::transposeInPlace();
|
|
}
|
|
|
|
void adjointInPlace()
|
|
requires(!is_immutable)
|
|
{
|
|
parent::adjointInPlace();
|
|
}
|
|
|
|
friend bool operator==(block const& lhs, block const& rhs)
|
|
{
|
|
return static_cast<parent const&>(lhs) == static_cast<parent const&>(rhs);
|
|
}
|
|
|
|
template<concepts::entity E>
|
|
block& operator+=(E const& rhs)
|
|
requires(!is_immutable)
|
|
{
|
|
base() += rhs.base();
|
|
return *this;
|
|
}
|
|
|
|
template<concepts::entity E>
|
|
block& operator-=(E const& rhs)
|
|
requires(!is_immutable)
|
|
{
|
|
base() -= rhs.base();
|
|
return *this;
|
|
}
|
|
|
|
concrete_type operator-() const
|
|
{
|
|
return concrete_type(static_cast<parent const&>(*this).operator-());
|
|
}
|
|
|
|
template<concepts::entity E>
|
|
block& operator*=(E const& rhs)
|
|
requires(!is_immutable)
|
|
{
|
|
base() *= rhs.base();
|
|
return *this;
|
|
}
|
|
|
|
block& operator*=(value_type rhs)
|
|
requires(!is_immutable)
|
|
{
|
|
base() *= rhs;
|
|
return *this;
|
|
}
|
|
|
|
block& operator/=(value_type rhs)
|
|
requires(!is_immutable)
|
|
{
|
|
base() /= rhs;
|
|
return *this;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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)
|
|
ROTGEN_ASSERT(rows == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_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)
|
|
ROTGEN_ASSERT(rows == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(cols == Cols,
|
|
"Mismatched between dynamic and static column size");
|
|
return parent::Ones(rows, cols);
|
|
}
|
|
|
|
static concrete_type Constant(value_type value)
|
|
requires(Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Constant(Rows, Cols, static_cast<double>(value));
|
|
}
|
|
|
|
static concrete_type Constant(int rows, int cols, value_type value)
|
|
{
|
|
if constexpr (Rows != -1)
|
|
ROTGEN_ASSERT(rows == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(cols == Cols,
|
|
"Mismatched between dynamic and static column size");
|
|
return parent::Constant(rows, cols, static_cast<double>(value));
|
|
}
|
|
|
|
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)
|
|
ROTGEN_ASSERT(rows == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(cols == Cols,
|
|
"Mismatched between dynamic and static column size");
|
|
return parent::Random(rows, cols);
|
|
}
|
|
|
|
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)
|
|
ROTGEN_ASSERT(rows == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(cols == Cols,
|
|
"Mismatched between dynamic and static column size");
|
|
return parent::Identity(rows, cols);
|
|
}
|
|
|
|
block& setOnes()
|
|
requires(!is_immutable)
|
|
{
|
|
parent::assign(parent::Ones(parent::rows(), parent::cols()));
|
|
return *this;
|
|
}
|
|
|
|
block& setZero()
|
|
requires(!is_immutable)
|
|
{
|
|
parent::assign(parent::Zero(parent::rows(), parent::cols()));
|
|
return *this;
|
|
}
|
|
|
|
block& setConstant(value_type value)
|
|
requires(!is_immutable)
|
|
{
|
|
parent::assign(parent::Constant(parent::rows(), parent::cols(), value));
|
|
return *this;
|
|
}
|
|
|
|
block& setRandom()
|
|
requires(!is_immutable)
|
|
{
|
|
parent::assign(parent::Random(parent::rows(), parent::cols()));
|
|
return *this;
|
|
}
|
|
|
|
block& setIdentity()
|
|
requires(!is_immutable)
|
|
{
|
|
parent::assign(parent::Identity(parent::rows(), parent::cols()));
|
|
return *this;
|
|
}
|
|
|
|
template<int P> value_type lpNorm() const
|
|
{
|
|
static_assert(P == 1 || P == 2 || P == Infinity);
|
|
return parent::lpNorm(P);
|
|
}
|
|
|
|
parent& base() { return static_cast<parent&>(*this); }
|
|
|
|
parent const& base() const { return static_cast<parent const&>(*this); }
|
|
};
|
|
|
|
template<typename Ref, int R, int C, bool I>
|
|
auto operator+(block<Ref, R, C, I> const& lhs, block<Ref, R, C, I> const& rhs)
|
|
{
|
|
using concrete_type = typename block<Ref, R, C, I>::concrete_type;
|
|
return concrete_type(lhs.base().add(rhs));
|
|
}
|
|
|
|
template<typename Ref, int R, int C, bool I>
|
|
auto operator-(block<Ref, R, C, I> const& lhs, block<Ref, R, C, I> const& rhs)
|
|
{
|
|
using concrete_type = typename block<Ref, R, C, I>::concrete_type;
|
|
return concrete_type(lhs.base().sub(rhs));
|
|
}
|
|
|
|
template<typename Ref, int R, int C, bool I>
|
|
auto operator*(block<Ref, R, C, I> const& lhs, block<Ref, R, C, I> const& rhs)
|
|
{
|
|
using concrete_type = typename block<Ref, R, C, I>::concrete_type;
|
|
return concrete_type(lhs.base().mul(rhs));
|
|
}
|
|
|
|
template<typename Ref, int R, int C, bool I>
|
|
auto operator*(block<Ref, R, C, I> const& lhs, double rhs)
|
|
{
|
|
using concrete_type = typename block<Ref, R, C, I>::concrete_type;
|
|
return concrete_type(lhs.base().mul(rhs));
|
|
}
|
|
|
|
template<typename Ref, int R, int C, bool I>
|
|
auto operator*(double lhs, block<Ref, R, C, I> const& rhs)
|
|
{
|
|
using concrete_type = typename block<Ref, R, C, I>::concrete_type;
|
|
return concrete_type(rhs.base().mul(lhs));
|
|
}
|
|
|
|
template<typename Ref, int R, int C, bool I>
|
|
auto operator/(block<Ref, R, C, I> const& lhs, double rhs)
|
|
{
|
|
using concrete_type = typename block<Ref, R, C, I>::concrete_type;
|
|
return concrete_type(lhs.base().div(rhs));
|
|
}
|
|
}
|