Space.h

Go to the documentation of this file.

#pragma once

 
#include <concepts>
#include <type_traits>
#include <utility>
 
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename S>
concept HasNormalize = requires(S const & space, Eigen::MatrixXd & block) { space.normalize(block.middleRows(0, 0)); };

template <typename S>
concept HasPlus = requires(S const & space, Eigen::MatrixXd & block) {
    space.plus(block.middleRows(0, 0), block.middleRows(0, 0), block.middleRows(0, 0));
};

template <typename S>
concept HasMinus = requires(S const & space, Eigen::MatrixXd & block) {
    space.minus(block.middleRows(0, 0), block.middleRows(0, 0), block.middleRows(0, 0));
};

template <typename S>
concept NearMissNormalize =
    !HasNormalize<S> && requires(S & space, Eigen::MatrixXd & block) { space.normalize(block); };

template <typename S>
concept NearMissPlus = !HasPlus<S> && requires(S & space, Eigen::MatrixXd & block) { space.plus(block, block, block); };

template <typename S>
concept NearMissMinus =
    !HasMinus<S> && requires(S & space, Eigen::MatrixXd & block) { space.minus(block, block, block); };

template <typename S>
concept NormalizeTakesBlocks = HasNormalize<S> || (!requires { &S::normalize; } && !NearMissNormalize<S>);

template <typename S>
concept PlusTakesBlocks = HasPlus<S> || (!requires { &S::plus; } && !NearMissPlus<S>);

template <typename S>
concept MinusTakesBlocks = HasMinus<S> || (!requires { &S::minus; } && !NearMissMinus<S>);

template <typename S>
concept HasDifference = requires(S const & space) { space.difference(); };

template <typename S>
struct DifferenceSpaceOf
{
    using type = S;
};

template <HasDifference S>
struct DifferenceSpaceOf<S>
{
    using type = std::remove_cvref_t<decltype(std::declval<S const &>().difference())>;
};

template <typename D, typename S>
concept DifferenceSpaceFor = std::regular<D> && requires(D const & d) {
    { d.valueDimension() } -> std::convertible_to<Eigen::Index>;
    typename std::integral_constant<int, D::FixedValueDimension>;
    typename std::integral_constant<int, D::ValueDimensionStep>;
    typename std::integral_constant<int, D::MaxValueDimension>;
    requires(D::ValueDimensionStep == 0) == (S::ValueDimensionStep == 0);
    requires(D::MaxValueDimension == Eigen::Dynamic) == (S::MaxValueDimension == Eigen::Dynamic);
};

template <typename S>
inline constexpr bool hasEqualSizes = DifferenceSpaceOf<S>::type::FixedValueDimension == S::FixedValueDimension
                                   && DifferenceSpaceOf<S>::type::ValueDimensionStep == S::ValueDimensionStep
                                   && DifferenceSpaceOf<S>::type::MaxValueDimension == S::MaxValueDimension;

template <typename S>
inline constexpr int resizableBlockCount = S::ValueDimensionStep == 0 ? 0 : 1;
 
}  // namespace internal

template <typename S>
concept Space = requires(S const & space) {
    typename std::integral_constant<int, S::FixedValueDimension>;
    typename std::integral_constant<int, S::ValueDimensionStep>;
    typename std::integral_constant<int, S::MaxValueDimension>;
    requires S::FixedValueDimension >= 0 && S::ValueDimensionStep >= 0;
    requires(
        S::ValueDimensionStep == 0
            ? S::MaxValueDimension == S::FixedValueDimension
            : (S::MaxValueDimension == Eigen::Dynamic || S::MaxValueDimension >= S::FixedValueDimension)
    );
    requires std::regular<S>;
    { space.valueDimension() } -> std::convertible_to<Eigen::Index>;
    requires internal::NormalizeTakesBlocks<S> && internal::PlusTakesBlocks<S> && internal::MinusTakesBlocks<S>;
    requires internal::NormalizeTakesBlocks<typename internal::DifferenceSpaceOf<S>::type>;
    requires internal::HasDifference<S>
                 || (!internal::HasNormalize<S> && !internal::HasPlus<S> && !internal::HasMinus<S>);
    requires internal::DifferenceSpaceFor<typename internal::DifferenceSpaceOf<S>::type, S>;
    requires internal::HasPlus<S> == internal::HasMinus<S>;
    requires internal::hasEqualSizes<S> || internal::HasPlus<S>;
    requires S::ValueDimensionStep == 0 || internal::resizableBlockCount<S> > 1
                     || requires(S & resizable, Eigen::Index n) { resizable.resize(n); };
};

template <typename S>
concept FixedSizeSpace = std::is_empty_v<S> && std::default_initializable<S>;

template <Space S>
using DifferenceSpace = typename internal::DifferenceSpaceOf<S>::type;

template <Space S>
[[nodiscard]] DifferenceSpace<S>
differenceSpace(S const & space)
{
    if constexpr (internal::HasDifference<S>)
    {
        return space.difference();
    }
    else
    {
        return space;
    }
}

template <typename Space>
inline constexpr int valueDimensionAtCompileTime =
    Space::ValueDimensionStep == 0 ? Space::FixedValueDimension : Eigen::Dynamic;

template <typename Space>
inline constexpr int maxValueDimensionAtCompileTime =
    Space::ValueDimensionStep == 0 ? Space::FixedValueDimension : Space::MaxValueDimension;

template <Space S>
[[nodiscard]] Eigen::Index
valueDimension(S const & space)
{
    return space.valueDimension();
}

template <Space S>
[[nodiscard]] Eigen::Index
differenceDimension(S const & space)
{
    if constexpr (internal::HasDifference<S>)
    {
        return space.difference().valueDimension();
    }
    else
    {
        return space.valueDimension();
    }
}

template <Space S>
inline constexpr bool hasPlainValues = !internal::HasNormalize<S> && !internal::HasPlus<S>;

template <Space S>
inline constexpr bool hasPlainDifferences =
    !internal::HasNormalize<DifferenceSpace<S>> && !internal::HasPlus<S> && !internal::HasMinus<S>;

template <Space S>
inline constexpr bool isLinear = hasPlainValues<S> && hasPlainDifferences<S>;

template <Space S, typename Derived>
void
normalizeValues(S const & space, Eigen::MatrixBase<Derived> const & block)
{
    if constexpr (internal::HasNormalize<S>)
    {
        space.normalize(block);
    }
}

template <Space S, typename Derived>
void
normalizeDifferences(S const & space, Eigen::MatrixBase<Derived> const & block)
{
    if constexpr (internal::HasNormalize<DifferenceSpace<S>>)
    {
        differenceSpace(space).normalize(block);
    }
}

template <FixedSizeSpace S, typename Derived>
    requires Space<S>
void
normalizeValues(Eigen::MatrixBase<Derived> const & block)
{
    normalizeValues(S{}, block);
}

template <FixedSizeSpace S, typename Derived>
    requires Space<S>
void
normalizeDifferences(Eigen::MatrixBase<Derived> const & block)
{
    normalizeDifferences(S{}, block);
}

template <typename Derived>
[[nodiscard]] Eigen::MatrixBase<Derived> &
writable(Eigen::MatrixBase<Derived> const & block)
{
    return const_cast<Eigen::MatrixBase<Derived> &>(block);
}
 
namespace internal
{
template <double Lower, double Period, typename Scalar>
[[nodiscard]] constexpr Scalar
wrapValue(Scalar x)
{
    constexpr double upper = Lower + Period;
    if (x >= Scalar(Lower) && x < Scalar(upper)) [[likely]]
    {
        return x;
    }
    auto y = Scalar{};
    if (x >= Scalar(Lower - Period) && x < Scalar(upper + Period)) [[likely]]
    {
        // Not a conditional expression, whose branches are distinct expression types for an
        // automatic differentiation scalar.
        if (x < Scalar(Lower))
        {
            y = x + Scalar(Period);
        }
        else
        {
            y = x - Scalar(Period);
        }
    }
    else
    {
        y = x - Scalar(Period) * floorOf<Scalar>((x - Scalar(Lower)) / Scalar(Period));
    }
    // Rounding can leave y an ulp or so below the lower bound: the quotient can round up to the next
    // integer, and Lower - Period is itself rounded. One more period brings it back. Just below the
    // lower bound, that sum, or the one above, can round up to the excluded upper bound. Written so
    // that NaN passes through.
    if (y < Scalar(Lower))
    {
        y += Scalar(Period);
    }
    return y >= Scalar(upper) ? Scalar(Lower) : y;
}

template <typename Scalar, int Rows, int Cols, int MaxRows, int MaxCols>
using MatrixOf = Eigen::
    Matrix<Scalar, Rows, Cols, (MaxRows == 1 && MaxCols != 1 ? Eigen::RowMajor : Eigen::ColMajor), MaxRows, MaxCols>;

template <typename L, typename R>
struct PairedColumns
{
    static constexpr bool broadcastLeft = L::ColsAtCompileTime == 1 && R::ColsAtCompileTime != 1;

    static constexpr bool broadcastRight = R::ColsAtCompileTime == 1 && L::ColsAtCompileTime != 1;

    static constexpr int cols = broadcastLeft ? int(R::ColsAtCompileTime) : int(L::ColsAtCompileTime);

    static constexpr int maxCols = broadcastLeft ? int(R::MaxColsAtCompileTime) : int(L::MaxColsAtCompileTime);

    template <typename Left, typename Right>
    [[nodiscard]] static Eigen::Index
    count(Left const & left, Right const & right)
    {
        EMRYS_FILTER_ASSERT(
            (broadcastLeft || broadcastRight || left.cols() == right.cols())
            && "the operands have different numbers of columns"
        );
        return broadcastLeft ? right.cols() : left.cols();
    }
};

template <bool Broadcast, typename Derived>
[[nodiscard]] decltype(auto)
withColumns(Eigen::MatrixBase<Derived> const & m, Eigen::Index cols)
{
    if constexpr (Broadcast)
    {
        return m.derived().replicate(1, cols);
    }
    else
    {
        return m.derived();
    }
}

template <typename Derived>
[[nodiscard]] decltype(auto)
inMemory(Eigen::MatrixBase<Derived> const & m)
{
    if constexpr ((Eigen::internal::traits<Derived>::Flags & Eigen::DirectAccessBit) != 0)
    {
        return m.derived();
    }
    else
    {
        return m.eval();
    }
}

template <typename S, typename X, typename D, typename Out>
void
plusInto(
    S const & space,
    Eigen::MatrixBase<X> const & x,
    Eigen::MatrixBase<D> const & d,
    Eigen::MatrixBase<Out> const & out
)
{
    if constexpr (HasPlus<S>)
    {
        space.plus(x, d, out);
    }
    else
    {
        writable(out) = x + d;
        normalizeValues(space, out);
    }
}

template <typename S, typename A, typename B, typename Out>
void
minusInto(
    S const & space,
    Eigen::MatrixBase<A> const & a,
    Eigen::MatrixBase<B> const & b,
    Eigen::MatrixBase<Out> const & out
)
{
    if constexpr (HasMinus<S>)
    {
        space.minus(a, b, out);
    }
    else
    {
        writable(out) = a - b;
        normalizeDifferences(space, out);
    }
}

template <bool Difference, typename S>
[[nodiscard]] Eigen::Index
rowsOf(S const & space)
{
    if constexpr (Difference)
    {
        return differenceSpace(space).valueDimension();
    }
    else
    {
        return space.valueDimension();
    }
}

template <bool Difference, typename S>
inline constexpr int rowsAtCompileTime =
    Difference ? valueDimensionAtCompileTime<DifferenceSpace<S>> : valueDimensionAtCompileTime<S>;

template <int Rows, typename M>
[[nodiscard]] auto
middleRowsOf(M & m, Eigen::Index offset, Eigen::Index rows)
{
    if constexpr (Rows == Eigen::Dynamic)
    {
        return m.middleRows(offset, rows);
    }
    else
    {
        EMRYS_FILTER_ASSERT(rows == Rows && "the block's rows must be its compile-time size");
        return m.template middleRows<Rows>(offset);
    }
}

template <int Cols, typename M>
[[nodiscard]] auto
middleColsOf(M & m, Eigen::Index offset, Eigen::Index cols)
{
    if constexpr (Cols == Eigen::Dynamic)
    {
        return m.middleCols(offset, cols);
    }
    else
    {
        EMRYS_FILTER_ASSERT(cols == Cols && "the block's columns must be its compile-time size");
        return m.template middleCols<Cols>(offset);
    }
}

template <int Rows, int Cols, typename M>
[[nodiscard]] auto
blockOf(M & m, Eigen::Index row, Eigen::Index col, Eigen::Index rows, Eigen::Index cols)
{
    EMRYS_FILTER_ASSERT(
        (Rows == Eigen::Dynamic || rows == Rows) && (Cols == Eigen::Dynamic || cols == Cols)
        && "the block's sizes must be its compile-time sizes"
    );
    return m.template block<Rows, Cols>(row, col, rows, cols);
}
 
}  // namespace internal

template <Space S, typename Scalar = double>
using ValueVector =
    Eigen::Matrix<Scalar, valueDimensionAtCompileTime<S>, 1, Eigen::ColMajor, maxValueDimensionAtCompileTime<S>, 1>;

template <Space S, typename Scalar = double>
using DifferenceVector = ValueVector<DifferenceSpace<S>, Scalar>;

template <Space Rows, Space Cols = Rows, typename Scalar = double>
using DifferenceMatrix = internal::MatrixOf<
    Scalar,
    valueDimensionAtCompileTime<DifferenceSpace<Rows>>,
    valueDimensionAtCompileTime<DifferenceSpace<Cols>>,
    maxValueDimensionAtCompileTime<DifferenceSpace<Rows>>,
    maxValueDimensionAtCompileTime<DifferenceSpace<Cols>>>;

template <Space S, typename XDerived, typename DDerived>
[[nodiscard]] auto
plus(S const & space, Eigen::MatrixBase<XDerived> const & x, Eigen::MatrixBase<DDerived> const & d)
{
    using Columns = internal::PairedColumns<XDerived, DDerived>;
    // The result's rows are the space's where it knows them, so that an unbounded operand into a
    // bounded space gives a bounded result.
    constexpr int rows = valueDimensionAtCompileTime<S> != Eigen::Dynamic ? valueDimensionAtCompileTime<S>
                                                                          : int(XDerived::RowsAtCompileTime);
    constexpr int maxRows = maxValueDimensionAtCompileTime<S> != Eigen::Dynamic ? maxValueDimensionAtCompileTime<S>
                                                                                : int(XDerived::MaxRowsAtCompileTime);
    using Result = internal::MatrixOf<typename XDerived::Scalar, rows, Columns::cols, maxRows, Columns::maxCols>;
    Eigen::Index const cols = Columns::count(x, d);
    auto result = Result{};
    result.resize(x.rows(), cols);
    // Evaluated once where they are expressions, so a space's plus() can read them by block or by
    // column without recomputing a product; a broadcast stays a lazy replicate of the evaluated operand.
    auto const & xInMemory = internal::inMemory(x);
    auto const & dInMemory = internal::inMemory(d);
    auto const & paired = internal::withColumns<Columns::broadcastLeft>(xInMemory, cols);
    auto const & offsets = internal::withColumns<Columns::broadcastRight>(dInMemory, cols);
    if constexpr (internal::HasPlus<S>)
    {
        space.plus(paired, offsets, result);
    }
    else
    {
        result = paired + offsets;
        normalizeValues(space, result);
    }
    return result;
}

template <Space S, typename ADerived, typename BDerived>
[[nodiscard]] auto
minus(S const & space, Eigen::MatrixBase<ADerived> const & a, Eigen::MatrixBase<BDerived> const & b)
{
    using Columns = internal::PairedColumns<ADerived, BDerived>;
    // The result's rows are the difference space's where it knows them, so that an unbounded
    // operand into a bounded space gives a bounded result, and the operands' rows otherwise, which
    // is right only where the space's differences have the sizes of its values.
    constexpr bool sameRows = internal::hasEqualSizes<S>;
    constexpr int spaceRows = valueDimensionAtCompileTime<DifferenceSpace<S>>;
    constexpr int spaceMaxRows = maxValueDimensionAtCompileTime<DifferenceSpace<S>>;
    constexpr int rows = spaceRows != Eigen::Dynamic ? spaceRows
                       : sameRows                    ? int(ADerived::RowsAtCompileTime)
                                                     : Eigen::Dynamic;
    constexpr int maxRows = spaceMaxRows != Eigen::Dynamic ? spaceMaxRows
                          : sameRows                       ? int(ADerived::MaxRowsAtCompileTime)
                                                           : Eigen::Dynamic;
    using Result = internal::MatrixOf<typename ADerived::Scalar, rows, Columns::cols, maxRows, Columns::maxCols>;
    Eigen::Index const cols = Columns::count(a, b);
    auto result = Result{};
    result.resize(sameRows ? a.rows() : differenceDimension(space), cols);
    auto const & aInMemory = internal::inMemory(a);
    auto const & bInMemory = internal::inMemory(b);
    auto const & left = internal::withColumns<Columns::broadcastLeft>(aInMemory, cols);
    auto const & right = internal::withColumns<Columns::broadcastRight>(bInMemory, cols);
    if constexpr (internal::HasMinus<S>)
    {
        space.minus(left, right, result);
    }
    else
    {
        result = left - right;
        normalizeDifferences(space, result);
    }
    return result;
}

template <Space S, typename XDerived, typename DDerived>
[[nodiscard]] auto
less(S const & space, Eigen::MatrixBase<XDerived> const & x, Eigen::MatrixBase<DDerived> const & d)
{
    return Emrys::plus(space, x, -d);
}

template <FixedSizeSpace S, typename XDerived, typename DDerived>
    requires Space<S>
[[nodiscard]] auto
plus(Eigen::MatrixBase<XDerived> const & x, Eigen::MatrixBase<DDerived> const & d)
{
    return Emrys::plus(S{}, x, d);
}

template <FixedSizeSpace S, typename ADerived, typename BDerived>
    requires Space<S>
[[nodiscard]] auto
minus(Eigen::MatrixBase<ADerived> const & a, Eigen::MatrixBase<BDerived> const & b)
{
    return Emrys::minus(S{}, a, b);
}

template <FixedSizeSpace S, typename XDerived, typename DDerived>
    requires Space<S>
[[nodiscard]] auto
less(Eigen::MatrixBase<XDerived> const & x, Eigen::MatrixBase<DDerived> const & d)
{
    return Emrys::less(S{}, x, d);
}

template <typename Space>
[[nodiscard]] constexpr bool
allowsValueDimension(Eigen::Index n)
{
    if constexpr (Space::ValueDimensionStep == 0)
    {
        return n == Space::FixedValueDimension;
    }
    else
    {
        return n >= Space::FixedValueDimension && (n - Space::FixedValueDimension) % Space::ValueDimensionStep == 0
            && (Space::MaxValueDimension == Eigen::Dynamic || n <= Space::MaxValueDimension);
    }
}
 
namespace internal
{

template <typename Space>
void
fitValueDimension(Space & space, Eigen::Index n)
{
    EMRYS_FILTER_ASSERT(allowsValueDimension<Space>(n) && "the dimension does not fit the space");
    if constexpr (requires { space.resize(n); })
    {
        if (space.valueDimension() != n)
        {
            space.resize(n);
        }
        EMRYS_FILTER_ASSERT(space.valueDimension() == n && "the space's resize() did not give it this dimension");
    }
    else
    {
        EMRYS_FILTER_ASSERT(space.valueDimension() == n && "the dimension does not fit the space");
    }
}
 
}  // namespace internal
 
}  // namespace Emrys