Math.h

Go to the documentation of this file.

#pragma once

 
#include <cmath>
#include <concepts>
#include <numbers>
#include <type_traits>
 
#include <EmrysFilter/Core/Macros.h>
 
#include <unsupported/Eigen/AutoDiff>
#include <unsupported/Eigen/SpecialFunctions>
 
namespace Emrys
{

template <typename T>
concept RealScalar =
    !Eigen::NumTraits<T>::IsInteger && !Eigen::NumTraits<T>::IsComplex && requires(T const & a, T const & b) {
        a + b;
        a - b;
        a * b;
        a / b;
        a < b;
    };
 
namespace internal
{

template <typename T>
inline constexpr bool isAutoDiffScalar = false;

template <typename Derivatives>
inline constexpr bool isAutoDiffScalar<Eigen::AutoDiffScalar<Derivatives>>{true};


template <typename... Values>
    requires(std::is_arithmetic_v<Values> && ...)
struct DeducedScalarOf
{
    using type = std::conditional_t<
        (std::integral<Values> && ...),
        double,
        std::common_type_t<std::conditional_t<std::integral<Values>, float, Values>...>>;
};

template <typename... Values>
using DeducedScalar = typename DeducedScalarOf<Values...>::type;
 
}  // namespace internal

template <typename Scalar>
[[nodiscard]] Scalar
pi()
{
    using Literal = typename Eigen::NumTraits<Scalar>::Literal;
    if constexpr (std::floating_point<Scalar>)
    {
        return std::numbers::pi_v<Scalar>;
    }
    else if constexpr (std::floating_point<Literal>)
    {
        return Scalar(std::numbers::pi_v<Literal>);
    }
    else
    {
        return Scalar(4) * Eigen::numext::atan(Scalar(1));
    }
}

template <typename Scalar>
[[nodiscard]] Scalar
log2Pi()
{
    using Literal = typename Eigen::NumTraits<Scalar>::Literal;
    if constexpr (std::floating_point<Scalar>)
    {
        return Scalar(1.83787706640934548356065947281123527972L);
    }
    else if constexpr (std::floating_point<Literal>)
    {
        return Scalar(log2Pi<Literal>());
    }
    else
    {
        Scalar const twoPi = Scalar(2) * pi<Scalar>();
        return Eigen::numext::log(twoPi);
    }
}

template <typename Scalar>
[[nodiscard]] Scalar
logGamma(std::type_identity_t<Scalar> const & x)
{
    if constexpr (std::floating_point<Scalar>)
    {
        return std::lgamma(x);
    }
    else if constexpr (internal::isAutoDiffScalar<Scalar>)
    {
        using Real = typename Scalar::Real;
        return Scalar{logGamma<Real>(x.value()), Eigen::numext::digamma(x.value()) * x.derivatives()};
    }
    else
    {
        return lgamma(x);
    }
}

template <typename Scalar>
[[nodiscard]] Scalar
logOnePlus(std::type_identity_t<Scalar> const & x)
{
    if constexpr (std::floating_point<Scalar>)
    {
        return std::log1p(x);
    }
    else if constexpr (internal::isAutoDiffScalar<Scalar>)
    {
        using Real = typename Scalar::Real;
        return Scalar{logOnePlus<Real>(x.value()), x.derivatives() / (Real(1) + x.value())};
    }
    else
    {
        return Eigen::numext::log1p(x);
    }
}

template <typename Scalar>
[[nodiscard]] Scalar
floorOf(std::type_identity_t<Scalar> const & x)
{
    if constexpr (std::floating_point<Scalar>)
    {
        return std::floor(x);
    }
    else if constexpr (internal::isAutoDiffScalar<Scalar>)
    {
        using Real = typename Scalar::Real;
        return Scalar{floorOf<Real>(x.value()), Scalar::DerType::Zero(x.derivatives().size())};
    }
    else
    {
        return Eigen::numext::floor(x);
    }
}

template <typename T>
[[nodiscard]] bool
isFinite(T const & x)
{
    if constexpr (std::floating_point<T>)
    {
        return std::isfinite(x);
    }
    else if constexpr (internal::isAutoDiffScalar<T>)
    {
        return isFinite(x.value());
    }
    else
    {
        return Eigen::numext::isfinite(x);
    }
}

template <typename T>
[[nodiscard]] bool
isNaN(T const & x)
{
    if constexpr (std::floating_point<T>)
    {
        return std::isnan(x);
    }
    else if constexpr (internal::isAutoDiffScalar<T>)
    {
        return isNaN(x.value());
    }
    else
    {
        return Eigen::numext::isnan(x);
    }
}

template <typename Derived>
[[nodiscard]] bool
allFinite(Eigen::DenseBase<Derived> const & x)
{
    if constexpr (std::floating_point<typename Derived::Scalar>)
    {
        return x.allFinite();
    }
    else
    {
        for (Eigen::Index j = 0; j < x.cols(); ++j)
        {
            for (Eigen::Index i = 0; i < x.rows(); ++i)
            {
                if (!isFinite(x.coeff(i, j)))
                {
                    return false;
                }
            }
        }
        return true;
    }
}
 
namespace internal
{

template <typename Scalar>
[[nodiscard]] Scalar
scalarFromCount(std::integral auto count)
{
    using Literal = typename Eigen::NumTraits<Scalar>::Literal;
    if constexpr (std::floating_point<Literal>)
    {
        return Scalar(Literal(count));
    }
    else
    {
        return Scalar(count);
    }
}

[[nodiscard]] consteval double
compileTimeSqrt(double x)
{
    if (x <= 0)
    {
        return 0;
    }
    double root = x > 1 ? x : 1;
    while (true)
    {
        double const next = (root + x / root) / 2;
        if (!(next < root))
        {
            return root;
        }
        root = next;
    }
}
 
}  // namespace internal
 
}  // namespace Emrys