407 lines
No EOL
12 KiB
C++
407 lines
No EOL
12 KiB
C++
//==================================================================================================
|
|
/*
|
|
ROTGEN - Runtime Overlay for Eigen
|
|
Copyright : CODE RECKONS
|
|
SPDX-License-Identifier: BSL-1.0
|
|
*/
|
|
//==================================================================================================
|
|
#pragma once
|
|
|
|
#include <rotgen/impl/matrix.hpp>
|
|
#include <initializer_list>
|
|
#include <cassert>
|
|
|
|
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 Index RowsAtCompileTime = Rows;
|
|
static constexpr Index ColsAtCompileTime = Cols;
|
|
static constexpr bool IsCompileTimeSized = Rows != -1 && Cols != -1;
|
|
static constexpr bool IsVectorAtCompileTime = (RowsAtCompileTime == 1) || (ColsAtCompileTime == 1);
|
|
static constexpr int Options = Opts;
|
|
static constexpr bool IsRowMajor = (Opts & RowMajor) == RowMajor;
|
|
static constexpr bool is_defined_static = false;
|
|
static constexpr bool has_static_storage = false;
|
|
|
|
using transposed_type = matrix<value_type,Cols,Rows, storage_order ^ RowMajor>;
|
|
|
|
matrix() : parent(Rows==-1?0:Rows,Cols==-1?0:Cols) {}
|
|
|
|
matrix(Index r, Index c) requires(!IsCompileTimeSized)
|
|
: 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; }
|
|
|
|
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()
|
|
{
|
|
*this = parent::Ones(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setOnes(int rows, int cols)
|
|
{
|
|
*this = parent::Ones(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
matrix& setZero()
|
|
{
|
|
*this = parent::Zero(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setZero(int rows, int cols)
|
|
{
|
|
*this = parent::Zero(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
matrix& setConstant(Scalar value)
|
|
{
|
|
*this = parent::Constant(parent::rows(), parent::cols(), static_cast<Scalar>(value));
|
|
return *this;
|
|
}
|
|
|
|
matrix& setConstant(int rows, int cols, Scalar value)
|
|
{
|
|
*this = parent::Constant(rows, cols, static_cast<Scalar>(value));
|
|
return *this;
|
|
}
|
|
|
|
matrix& setRandom()
|
|
{
|
|
*this = parent::Random(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setRandom(int rows, int cols)
|
|
{
|
|
*this = parent::Random(rows, cols);
|
|
return *this;
|
|
}
|
|
|
|
matrix& setIdentity()
|
|
{
|
|
*this = parent::Identity(parent::rows(), parent::cols());
|
|
return *this;
|
|
}
|
|
|
|
matrix& setIdentity(int rows, int cols)
|
|
{
|
|
*this = parent::Identity(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;
|
|
}
|
|
} |