473 lines
13 KiB
C++
473 lines
13 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/detail/helpers.hpp>
|
|
|
|
#include <rotgen/concepts.hpp>
|
|
#include <rotgen/container/matrix/dynamic/impl.hpp>
|
|
|
|
#include <initializer_list>
|
|
|
|
namespace rotgen
|
|
{
|
|
template<typename Scalar,
|
|
int Rows = Dynamic,
|
|
int Cols = Dynamic,
|
|
int Opts = detail::force_order<Rows, Cols>,
|
|
int MaxRows = Rows,
|
|
int MaxCols = Cols>
|
|
class matrix : public find_matrix<Scalar, Opts>
|
|
{
|
|
public:
|
|
using parent = find_matrix<Scalar, Opts>;
|
|
using rotgen_tag = void;
|
|
using rotgen_matrix_tag = void;
|
|
using concrete_type = matrix;
|
|
using value_type = Scalar;
|
|
|
|
static constexpr auto storage_order = Opts & 1;
|
|
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 = (Rows == 1) || (Cols == 1);
|
|
static constexpr int Options = Opts;
|
|
static constexpr bool IsRowMajor = (Opts & RowMajor) == RowMajor;
|
|
static constexpr int InnerSizeAtCompileTime =
|
|
IsVectorAtCompileTime ? SizeAtCompileTime
|
|
: IsRowMajor ? ColsAtCompileTime
|
|
: RowsAtCompileTime;
|
|
|
|
static constexpr bool is_defined_static = Rows != -1 && Cols != -1;
|
|
static constexpr bool has_static_storage = false;
|
|
static constexpr bool is_immutable = false;
|
|
static constexpr int InnerStrideAtCompileTime = 1;
|
|
static constexpr int OuterStrideAtCompileTime = IsRowMajor ? Cols : Rows;
|
|
|
|
using transposed_type = matrix<value_type, Cols, Rows, storage_order>;
|
|
|
|
static constexpr int AllocatedRows =
|
|
Rows == -1 ? (MaxRows == -1 ? 0 : MaxRows) : Rows;
|
|
static constexpr int AllocatedCols =
|
|
Cols == -1 ? (MaxCols == -1 ? 0 : MaxCols) : Cols;
|
|
|
|
matrix() : parent(AllocatedRows, AllocatedCols) {}
|
|
|
|
matrix(Index r, Index c) : parent(r, c)
|
|
{
|
|
if constexpr (Rows != -1)
|
|
ROTGEN_ASSERT(r == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(c == Cols,
|
|
"Mismatched between dynamic and static column size");
|
|
}
|
|
|
|
matrix(Index n)
|
|
requires(IsVectorAtCompileTime && (Rows != 1 || Cols != 1))
|
|
: parent(Rows != 1 ? n : 1, Cols != 1 ? n : 1)
|
|
{
|
|
if constexpr (Rows == 1 && Cols != -1)
|
|
ROTGEN_ASSERT(Cols == n,
|
|
"Mismatched between dynamic and static col size");
|
|
if constexpr (Cols == 1 && Rows != -1)
|
|
ROTGEN_ASSERT(Rows == n,
|
|
"Mismatched between dynamic and static row size");
|
|
}
|
|
|
|
explicit matrix(Scalar v)
|
|
requires(Rows == 1 && Cols == 1)
|
|
: parent(1, 1, {v})
|
|
{
|
|
}
|
|
|
|
matrix(Scalar v0, Scalar v1, auto... vs)
|
|
requires((Rows == (2 + sizeof...(vs)) && Cols == 1) ||
|
|
(Rows == 1 && Cols == (2 + sizeof...(vs))))
|
|
: parent(Rows, Cols, {v0, v1, static_cast<Scalar>(vs)...})
|
|
{
|
|
}
|
|
|
|
matrix(parent const& base) : parent(base) {}
|
|
|
|
matrix(std::initializer_list<std::initializer_list<Scalar>> init)
|
|
: parent(init)
|
|
{
|
|
if constexpr (Rows != -1)
|
|
ROTGEN_ASSERT(init.size() == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
{
|
|
[[maybe_unused]] Index c = 0;
|
|
if (init.size()) c = init.begin()->size();
|
|
ROTGEN_ASSERT(c == Cols,
|
|
"Mismatched between dynamic and static column size");
|
|
}
|
|
}
|
|
|
|
matrix(std::initializer_list<Scalar> init)
|
|
requires(IsVectorAtCompileTime)
|
|
: parent(Rows, Cols, init)
|
|
{
|
|
}
|
|
|
|
template<concepts::entity Src> matrix(Src const& other) : parent()
|
|
{
|
|
if constexpr (IsVectorAtCompileTime && Src::IsVectorAtCompileTime)
|
|
{
|
|
resize(other.size());
|
|
for (rotgen::Index i = 0; i < parent::size(); ++i)
|
|
(*this)[i] = other[i];
|
|
}
|
|
else
|
|
{
|
|
resize(other.rows(), other.cols());
|
|
|
|
for (rotgen::Index r = 0; r < parent::rows(); ++r)
|
|
for (rotgen::Index c = 0; c < parent::cols(); ++c)
|
|
(*this)(r, c) = other(r, c);
|
|
}
|
|
}
|
|
|
|
matrix& operator=(concepts::entity auto const& other)
|
|
{
|
|
matrix local(other);
|
|
swap(local);
|
|
return *this;
|
|
}
|
|
|
|
value_type& operator[](Index i)
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
value_type operator[](Index i) const
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
void swap(matrix& other)
|
|
{
|
|
// TODO: Swap elements per elements if statically defined to preserve
|
|
// data location in memory as with actual statically defines matrix
|
|
base().swap(other.base());
|
|
}
|
|
|
|
auto evaluate() const { return *this; }
|
|
|
|
decltype(auto) noalias() const { return *this; }
|
|
|
|
decltype(auto) noalias() { return *this; }
|
|
|
|
Index innerStride() const noexcept { return 1; }
|
|
|
|
Index outerStride() const noexcept
|
|
{
|
|
if constexpr (IsVectorAtCompileTime) return this->size();
|
|
else
|
|
{
|
|
if constexpr (IsRowMajor) return this->cols();
|
|
else return this->rows();
|
|
}
|
|
}
|
|
|
|
void resize(int r, int c)
|
|
{
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(c == Cols,
|
|
"Mismatched between dynamic and static col size");
|
|
if constexpr (Rows != -1)
|
|
ROTGEN_ASSERT(r == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
parent::resize(r, c);
|
|
}
|
|
|
|
void resize(int s)
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
if constexpr (Rows == 1) parent::resize(1, s);
|
|
else parent::resize(s, 1);
|
|
}
|
|
|
|
void conservativeResize(int r, int c)
|
|
{
|
|
if constexpr (Cols != -1)
|
|
ROTGEN_ASSERT(c == Cols,
|
|
"Mismatched between dynamic and static col size");
|
|
if constexpr (Rows != -1)
|
|
ROTGEN_ASSERT(r == Rows,
|
|
"Mismatched between dynamic and static row size");
|
|
parent::conservativeResize(r, c);
|
|
}
|
|
|
|
void conservativeResize(int s)
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
if constexpr (Rows == 1) parent::conservativeResize(1, s);
|
|
else parent::conservativeResize(s, 1);
|
|
}
|
|
|
|
matrix normalized() const
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return matrix(base().normalized());
|
|
}
|
|
|
|
transposed_type transpose() const
|
|
{
|
|
return transposed_type(base().transpose());
|
|
}
|
|
|
|
matrix conjugate() const { return matrix(base().conjugate()); }
|
|
|
|
transposed_type adjoint() const
|
|
{
|
|
return transposed_type(base().adjoint());
|
|
}
|
|
|
|
void normalize()
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
parent::normalize();
|
|
}
|
|
|
|
void transposeInPlace() { parent::transposeInPlace(); }
|
|
|
|
void adjointInPlace() { parent::adjointInPlace(); }
|
|
|
|
matrix cwiseAbs() const { return matrix(base().cwiseAbs()); }
|
|
|
|
matrix cwiseAbs2() const { return matrix(base().cwiseAbs2()); }
|
|
|
|
matrix cwiseInverse() const { return matrix(base().cwiseInverse()); }
|
|
|
|
matrix cwiseSqrt() const { return matrix(base().cwiseSqrt()); }
|
|
|
|
friend bool operator==(matrix const& lhs, matrix const& rhs)
|
|
{
|
|
return lhs.base() == rhs.base();
|
|
}
|
|
|
|
matrix& operator+=(matrix const& rhs)
|
|
{
|
|
base() += rhs.base();
|
|
return *this;
|
|
}
|
|
|
|
matrix& operator-=(matrix const& rhs)
|
|
{
|
|
base() -= rhs.base();
|
|
return *this;
|
|
}
|
|
|
|
matrix operator-() const { return -base(); }
|
|
|
|
matrix& operator*=(matrix const& rhs)
|
|
{
|
|
base() *= rhs.base();
|
|
return *this;
|
|
}
|
|
|
|
matrix& operator*=(Scalar rhs)
|
|
{
|
|
base() *= rhs;
|
|
return *this;
|
|
}
|
|
|
|
matrix& operator/=(Scalar rhs)
|
|
{
|
|
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 matrix Ones()
|
|
requires(Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Ones(Rows, Cols);
|
|
}
|
|
|
|
static matrix 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 matrix Zero()
|
|
requires(Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Zero(Rows, Cols);
|
|
}
|
|
|
|
static matrix 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 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)
|
|
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<Scalar>(value));
|
|
}
|
|
|
|
static matrix Random()
|
|
requires(Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Random(Rows, Cols);
|
|
}
|
|
|
|
static matrix 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 matrix Identity()
|
|
requires(Rows != -1 && Cols != -1)
|
|
{
|
|
return parent::Identity(Rows, Cols);
|
|
}
|
|
|
|
static matrix 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);
|
|
}
|
|
|
|
matrix& setOnes()
|
|
{
|
|
parent::setOnes(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setOnes(int rows, int cols)
|
|
{
|
|
parent::setOnes(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
matrix& setZero()
|
|
{
|
|
parent::setZero(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setZero(int rows, int cols)
|
|
{
|
|
parent::setZero(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
matrix& setConstant(Scalar v)
|
|
{
|
|
parent::setConstant(parent::rows(), parent::cols(),
|
|
static_cast<Scalar>(v));
|
|
return *this;
|
|
}
|
|
|
|
matrix& setConstant(int rows, int cols, Scalar v)
|
|
{
|
|
parent::setConstant(rows, cols, static_cast<Scalar>(v));
|
|
return *this;
|
|
}
|
|
|
|
matrix& setRandom()
|
|
{
|
|
parent::setRandom(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setRandom(int rows, int cols)
|
|
{
|
|
parent::setRandom(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
matrix& setIdentity()
|
|
{
|
|
parent::setIdentity(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setIdentity(int rows, int cols)
|
|
{
|
|
parent::setIdentity(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
template<int P> Scalar lpNorm() const
|
|
{
|
|
static_assert(P == 1 || P == 2 || P == Infinity);
|
|
return parent::lp_norm(P);
|
|
}
|
|
|
|
parent& base() { return static_cast<parent&>(*this); }
|
|
|
|
parent const& base() const { return static_cast<parent const&>(*this); }
|
|
};
|
|
}
|