501 lines
14 KiB
C++
501 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 <rotgen/concepts.hpp>
|
|
#include <rotgen/container/matrix/dynamic/impl.hpp>
|
|
|
|
#include <cassert>
|
|
#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 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 ? ColsAtCompileTime : RowsAtCompileTime;
|
|
|
|
using transposed_type = matrix<value_type, Cols, Rows, storage_order>;
|
|
|
|
matrix() : parent(Rows == -1 ? 0 : Rows, Cols == -1 ? 0 : Cols) {}
|
|
|
|
matrix(Index r, Index c) : parent(r, c)
|
|
{
|
|
if constexpr (Rows != -1)
|
|
assert(r == Rows && "Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
assert(c == Cols &&
|
|
"Mismatched between dynamic and static column size");
|
|
}
|
|
|
|
matrix(Index n)
|
|
requires(IsVectorAtCompileTime && (Rows != 1 || Cols != 1))
|
|
: parent(Rows != -1 ? 1 : n, Cols != -1 ? 1 : n)
|
|
{
|
|
}
|
|
|
|
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)
|
|
assert(init.size() == Rows &&
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
{
|
|
[[maybe_unused]] std::size_t c = 0;
|
|
if (init.size()) c = init.begin()->size();
|
|
assert(c == Cols &&
|
|
"Mismatched between dynamic and static column size");
|
|
}
|
|
}
|
|
|
|
matrix(std::initializer_list<Scalar> init)
|
|
requires(IsVectorAtCompileTime)
|
|
: parent(Rows, Cols, init)
|
|
{
|
|
}
|
|
|
|
matrix(concepts::entity auto const& e) : parent(e.rows(), e.cols())
|
|
{
|
|
if constexpr (Rows != -1)
|
|
assert(e.rows() == Rows &&
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
assert(e.cols() == Cols &&
|
|
"Mismatched between dynamic and static col size");
|
|
for (rotgen::Index r = 0; r < e.rows(); ++r)
|
|
for (rotgen::Index c = 0; c < e.cols(); ++c) (*this)(r, c) = e(r, c);
|
|
}
|
|
|
|
matrix& operator=(concepts::entity auto const& e)
|
|
{
|
|
if constexpr (Rows != -1)
|
|
assert(e.rows() == Rows &&
|
|
"Mismatched between dynamic and static row size");
|
|
if constexpr (Cols != -1)
|
|
assert(e.cols() == Cols &&
|
|
"Mismatched between dynamic and static col size");
|
|
resize(e.rows(), e.cols());
|
|
|
|
for (rotgen::Index r = 0; r < e.rows(); ++r)
|
|
for (rotgen::Index c = 0; c < e.cols(); ++c) (*this)(r, c) = e(r, c);
|
|
|
|
return *this;
|
|
}
|
|
|
|
value_type& operator[](Index i)
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
value_type operator[](Index i) const
|
|
requires(IsVectorAtCompileTime)
|
|
{
|
|
return (*this)(i);
|
|
}
|
|
|
|
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
|
|
{
|
|
return IsVectorAtCompileTime ? this->size()
|
|
: IsRowMajor ? this->cols()
|
|
: this->rows();
|
|
}
|
|
|
|
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 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 (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);
|
|
}
|
|
|
|
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 static_cast<parent const&>(lhs) == static_cast<parent const&>(rhs);
|
|
}
|
|
|
|
matrix& operator+=(matrix const& rhs)
|
|
{
|
|
base() += static_cast<parent const&>(rhs);
|
|
return *this;
|
|
}
|
|
|
|
matrix& operator-=(matrix const& rhs)
|
|
{
|
|
base() -= static_cast<parent const&>(rhs);
|
|
return *this;
|
|
}
|
|
|
|
matrix operator-() const { return matrix(base().operator-()); }
|
|
|
|
matrix& operator*=(matrix const& rhs)
|
|
{
|
|
base() *= static_cast<parent const&>(rhs);
|
|
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)
|
|
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 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 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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
;
|
|
}
|
|
};
|
|
|
|
template<typename S, int R, int C, int O, int MR, int MC>
|
|
matrix<S, R, C, O, MR, MC> operator+(matrix<S, R, C, O, MR, MC> const& lhs,
|
|
matrix<S, R, C, O, MR, MC> const& rhs)
|
|
{
|
|
matrix<S, R, C, O, MR, MC> that(lhs);
|
|
return that += rhs;
|
|
}
|
|
|
|
template<typename S, int R, int C, int O, int MR, int MC>
|
|
matrix<S, R, C, O, MR, MC> operator-(matrix<S, R, C, O, MR, MC> const& lhs,
|
|
matrix<S, R, C, O, MR, MC> const& rhs)
|
|
{
|
|
matrix<S, R, C, O, MR, MC> that(lhs);
|
|
return that -= rhs;
|
|
}
|
|
|
|
template<typename S, int R, int C, int O, int MR, int MC>
|
|
matrix<S, R, C, O, MR, MC> operator*(matrix<S, R, C, O, MR, MC> const& lhs,
|
|
matrix<S, R, C, O, MR, MC> const& rhs)
|
|
{
|
|
matrix<S, R, C, O, MR, MC> that(lhs);
|
|
return that *= rhs;
|
|
}
|
|
|
|
template<typename S, int R, int C, int O, int MR, int MC>
|
|
matrix<S, R, C, O, MR, MC> operator*(matrix<S, R, C, O, MR, MC> const& lhs,
|
|
double rhs)
|
|
{
|
|
matrix<S, R, C, O, MR, MC> that(lhs);
|
|
return that *= rhs;
|
|
}
|
|
|
|
template<typename S, int R, int C, int O, int MR, int MC>
|
|
matrix<S, R, C, O, MR, MC> operator*(double lhs,
|
|
matrix<S, R, C, O, MR, MC> const& rhs)
|
|
{
|
|
return rhs * lhs;
|
|
}
|
|
|
|
template<typename S, int R, int C, int O, int MR, int MC>
|
|
matrix<S, R, C, O, MR, MC> operator/(matrix<S, R, C, O, MR, MC> const& lhs,
|
|
double rhs)
|
|
{
|
|
matrix<S, R, C, O, MR, MC> that(lhs);
|
|
return that /= rhs;
|
|
}
|
|
}
|