541 lines
14 KiB
C++
541 lines
14 KiB
C++
//==================================================================================================
|
|
/*
|
|
ROTGEN - Runtime Overlay for Eigen
|
|
Copyright : CODE RECKONS
|
|
SPDX-License-Identifier: BSL-1.0
|
|
*/
|
|
//==================================================================================================
|
|
#pragma once
|
|
|
|
#include <rotgen/detail/helpers.hpp>
|
|
|
|
#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 int RowsAtCompileTime = Rows;
|
|
static constexpr int ColsAtCompileTime = Cols;
|
|
static constexpr int SizeAtCompileTime = detail::static_size<matrix>();
|
|
static constexpr int MaxRowsAtCompileTime = MaxRows;
|
|
static constexpr int MaxColsAtCompileTime = MaxCols;
|
|
static constexpr bool IsVectorAtCompileTime =
|
|
(RowsAtCompileTime == 1) || (ColsAtCompileTime == 1);
|
|
static constexpr int Options = parent::Options;
|
|
static constexpr bool IsRowMajor = parent::IsRowMajor;
|
|
static constexpr int InnerStrideAtCompileTime = 1;
|
|
static constexpr int OuterStrideAtCompileTime =
|
|
IsRowMajor ? ColsAtCompileTime : RowsAtCompileTime;
|
|
|
|
static constexpr int InnerSizeAtCompileTime =
|
|
IsVectorAtCompileTime ? SizeAtCompileTime
|
|
: IsRowMajor ? ColsAtCompileTime
|
|
: RowsAtCompileTime;
|
|
|
|
template<typename ET>
|
|
using as_concrete_type = detail::as_concrete_t<ET, matrix>;
|
|
|
|
static constexpr bool is_immutable = false;
|
|
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)
|
|
{
|
|
if constexpr (RowsAtCompileTime != -1)
|
|
assert(r == RowsAtCompileTime &&
|
|
"Mismatched between dynamic and static row size");
|
|
|
|
if constexpr (ColsAtCompileTime != -1)
|
|
assert(c == ColsAtCompileTime &&
|
|
"Mismatched between dynamic and static column size");
|
|
}
|
|
|
|
matrix(matrix const& other) = default;
|
|
matrix(matrix&& other) = default;
|
|
matrix& operator=(matrix const&) = 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)
|
|
{
|
|
}
|
|
|
|
explicit 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;
|
|
}
|
|
|
|
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(Scalar v0, Scalar v1, auto... vs)
|
|
requires((Rows == (2 + sizeof...(vs)) && Cols == 1) ||
|
|
(Rows == 1 && Cols == (2 + sizeof...(vs))))
|
|
: matrix({v0, v1, static_cast<Scalar>(vs)...})
|
|
{
|
|
}
|
|
|
|
matrix(concepts::entity auto const& other) : parent(other.base()) {}
|
|
|
|
template<typename OtherDerived>
|
|
matrix(Eigen::MatrixBase<OtherDerived> const& other) : parent(other)
|
|
{
|
|
}
|
|
|
|
template<typename OtherDerived>
|
|
matrix(Eigen::EigenBase<OtherDerived> const& other) : parent(other)
|
|
{
|
|
}
|
|
|
|
template<typename OtherDerived>
|
|
matrix& operator=(Eigen::MatrixBase<OtherDerived> const& other)
|
|
{
|
|
parent::operator=(other);
|
|
return *this;
|
|
}
|
|
|
|
template<typename OtherDerived>
|
|
matrix& operator=(Eigen::EigenBase<OtherDerived> const& other)
|
|
{
|
|
parent::operator=(other);
|
|
return *this;
|
|
}
|
|
|
|
matrix& operator=(concepts::entity auto const& other)
|
|
{
|
|
parent::operator=(other.base());
|
|
return *this;
|
|
}
|
|
|
|
using parent::innerStride;
|
|
using parent::outerStride;
|
|
|
|
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 s)
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
if constexpr (Rows == 1)
|
|
assert(s == Cols && "Mismatched between dynamic and static col size");
|
|
if constexpr (Cols == 1)
|
|
assert(s == Rows && "Mismatched between dynamic and static row size");
|
|
parent::resize(s);
|
|
}
|
|
|
|
void resize(int r, int c)
|
|
{
|
|
if constexpr (Rows == 1)
|
|
assert(c == Cols && "Mismatched between dynamic and static col size");
|
|
if constexpr (Cols == 1)
|
|
assert(r == Rows && "Mismatched between dynamic and static row size");
|
|
parent::resize(r, c);
|
|
}
|
|
|
|
void conservativeResize(int s)
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
if constexpr (Rows == 1)
|
|
assert(s == Cols && "Mismatched between dynamic and static col size");
|
|
if constexpr (Cols == 1)
|
|
assert(s == Rows && "Mismatched between dynamic and static row size");
|
|
parent::conservativeResize(s);
|
|
}
|
|
|
|
void conservativeResize(int r, int c)
|
|
{
|
|
if constexpr (Rows == 1)
|
|
assert(c == Cols && "Mismatched between dynamic and static col size");
|
|
if constexpr (Cols == 1)
|
|
assert(r == Rows && "Mismatched between dynamic and static row size");
|
|
parent::conservativeResize(r, c);
|
|
}
|
|
|
|
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::cols;
|
|
using parent::data;
|
|
using parent::mean;
|
|
using parent::norm;
|
|
using parent::prod;
|
|
using parent::rows;
|
|
using parent::size;
|
|
using parent::squaredNorm;
|
|
using parent::sum;
|
|
using parent::trace;
|
|
|
|
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;
|
|
}
|
|
};
|
|
}
|