Arithmetic.hpp

Go to the documentation of this file.

#pragma once
 
#include "janus/core/JanusConcepts.hpp"
#include <Eigen/Dense>
#include <cmath>
 
namespace janus {
 
// --- Absolute Value ---
template <JanusScalar T> T abs(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::abs(x);
    } else {
        return fabs(x);
    }
}

template <typename Derived> auto abs(const Eigen::MatrixBase<Derived> &x) {
    return x.array().abs().matrix();
}
 
// --- Square Root ---
template <JanusScalar T> T sqrt(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::sqrt(x);
    } else {
        return sqrt(x);
    }
}

template <typename Derived> auto sqrt(const Eigen::MatrixBase<Derived> &x) {
    return x.array().sqrt().matrix();
}
 
// --- Power ---
template <JanusScalar T> T pow(const T &base, const T &exponent) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::pow(base, exponent);
    } else {
        return pow(base, exponent);
    }
}

template <JanusScalar T>
    requires(!std::is_same_v<T, double>)
T pow(const T &base, double exponent) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::pow(base, static_cast<T>(exponent));
    } else {
        return janus::pow(base, static_cast<T>(exponent));
    }
}

template <JanusScalar T>
    requires(!std::is_same_v<T, double>)
T pow(double base, const T &exponent) {
    // Cast base to T to match homogeneous overload
    return janus::pow(static_cast<T>(base), exponent);
}

template <typename Derived, typename Scalar>
auto pow(const Eigen::MatrixBase<Derived> &base, const Scalar &exponent) {
    return base.array().pow(exponent).matrix();
}
 
// --- Exp ---
template <JanusScalar T> T exp(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::exp(x);
    } else {
        return exp(x);
    }
}

template <typename Derived> auto exp(const Eigen::MatrixBase<Derived> &x) {
    return x.array().exp().matrix();
}
 
// --- Log ---
template <JanusScalar T> T log(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::log(x);
    } else {
        return log(x);
    }
}

template <typename Derived> auto log(const Eigen::MatrixBase<Derived> &x) {
    return x.array().log().matrix();
}

template <JanusScalar T> T log10(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::log10(x);
    } else {
        return log10(x);
    }
}

template <typename Derived> auto log10(const Eigen::MatrixBase<Derived> &x) {
    return x.array().log10().matrix();
}
 
// --- Hyperbolic Functions ---
template <JanusScalar T> T sinh(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::sinh(x);
    } else {
        return sinh(x);
    }
}

template <typename Derived> auto sinh(const Eigen::MatrixBase<Derived> &x) {
    return x.array().sinh().matrix();
}

template <JanusScalar T> T cosh(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::cosh(x);
    } else {
        return cosh(x);
    }
}

template <typename Derived> auto cosh(const Eigen::MatrixBase<Derived> &x) {
    return x.array().cosh().matrix();
}

template <JanusScalar T> T tanh(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::tanh(x);
    } else {
        return tanh(x);
    }
}

template <typename Derived> auto tanh(const Eigen::MatrixBase<Derived> &x) {
    return x.array().tanh().matrix();
}
 
// --- Rounding and Sign ---
template <JanusScalar T> T floor(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::floor(x);
    } else {
        return floor(x);
    }
}

template <typename Derived> auto floor(const Eigen::MatrixBase<Derived> &x) {
    return x.array().floor().matrix();
}

template <JanusScalar T> T ceil(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::ceil(x);
    } else {
        return ceil(x);
    }
}

template <typename Derived> auto ceil(const Eigen::MatrixBase<Derived> &x) {
    return x.array().ceil().matrix();
}

template <JanusScalar T> T sign(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        // Return 1.0, -1.0, or 0.0
        return (x > 0) ? T(1.0) : ((x < 0) ? T(-1.0) : T(0.0));
    } else {
        return sign(x);
    }
}

template <typename Derived> auto sign(const Eigen::MatrixBase<Derived> &x) {
    return x.array().sign().matrix();
}
 
// --- Modulo ---
template <JanusScalar T> T fmod(const T &x, const T &y) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::fmod(x, y);
    } else {
        return fmod(x, y);
    }
}
 
// Note: Ensure strictly positive modulus if needed, std::fmod matches C++ behavior.
// There is no direct .fmod() in Eigen Array, need to map
template <typename Derived, typename Scalar>
auto fmod(const Eigen::MatrixBase<Derived> &x, const Scalar &y) {
    if constexpr (std::is_same_v<typename Derived::Scalar, casadi::MX> ||
                  std::is_same_v<Scalar, casadi::MX>) {
        // Element-wise fmod when either operand is symbolic
        using ResultScalar = casadi::MX;
        Eigen::Matrix<ResultScalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> result(
            x.rows(), x.cols());
        const ResultScalar y_mx = static_cast<ResultScalar>(y);
        for (Eigen::Index i = 0; i < x.rows(); ++i) {
            for (Eigen::Index j = 0; j < x.cols(); ++j) {
                result(i, j) = janus::fmod(static_cast<ResultScalar>(x(i, j)), y_mx);
            }
        }
        return result;
    } else {
        return x.binaryExpr(Eigen::MatrixBase<Derived>::Constant(x.rows(), x.cols(), y),
                            [](double a, double b) { return std::fmod(a, b); });
    }
}
 
// --- Log2 ---
template <JanusScalar T> T log2(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::log2(x);
    } else {
        // CasADi: log2(x) = log(x) / log(2)
        return log(x) / log(T(2.0));
    }
}

template <typename Derived> auto log2(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::log2(v); });
    } else {
        return (x.array().log() / std::log(2.0)).matrix();
    }
}
 
// --- Exp2 ---
template <JanusScalar T> T exp2(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::exp2(x);
    } else {
        // CasADi: 2^x = exp(x * log(2))
        return exp(x * log(T(2.0)));
    }
}

template <typename Derived> auto exp2(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::exp2(v); });
    } else {
        return (x.array() * std::log(2.0)).exp().matrix();
    }
}
 
// --- Cbrt ---
template <JanusScalar T> T cbrt(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::cbrt(x);
    } else {
        // CasADi: cbrt(x) = sign(x) * pow(abs(x), 1/3)
        // This handles negative values correctly
        return sign(x) * pow(fabs(x), T(1.0 / 3.0));
    }
}

template <typename Derived> auto cbrt(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::cbrt(v); });
    } else {
        return x.unaryExpr([](double v) { return std::cbrt(v); });
    }
}
 
// --- Round ---
template <JanusScalar T> T round(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::round(x);
    } else {
        // CasADi: implement round as floor(x + 0.5) for positive,
        // ceil(x - 0.5) for negative (round half away from zero)
        return floor(x + T(0.5));
    }
}

template <typename Derived> auto round(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::round(v); });
    } else {
        return x.array().round().matrix();
    }
}
 
// --- Trunc ---
template <JanusScalar T> T trunc(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::trunc(x);
    } else {
        // CasADi: trunc(x) = sign(x) * floor(abs(x))
        return sign(x) * floor(fabs(x));
    }
}

template <typename Derived> auto trunc(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::trunc(v); });
    } else {
        return x.unaryExpr([](double v) { return std::trunc(v); });
    }
}
 
// --- Hypot ---
template <JanusScalar T> T hypot(const T &x, const T &y) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::hypot(x, y);
    } else {
        // CasADi: use sqrt(x^2 + y^2) - CasADi handles this symbolically
        return sqrt(x * x + y * y);
    }
}

template <JanusScalar T>
    requires(!std::is_same_v<T, double>)
T hypot(const T &x, double y) {
    return janus::hypot(x, T(y));
}

template <JanusScalar T>
    requires(!std::is_same_v<T, double>)
T hypot(double x, const T &y) {
    return janus::hypot(T(x), y);
}

template <typename Derived>
auto hypot(const Eigen::MatrixBase<Derived> &x, const Eigen::MatrixBase<Derived> &y) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return (x.array().square() + y.array().square()).sqrt().matrix();
    } else {
        return x.binaryExpr(y, [](double a, double b) { return std::hypot(a, b); });
    }
}
 
// --- Expm1 ---
template <JanusScalar T> T expm1(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::expm1(x);
    } else {
        // CasADi: fall back to exp(x) - 1
        return exp(x) - T(1.0);
    }
}

template <typename Derived> auto expm1(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::expm1(v); });
    } else {
        return x.unaryExpr([](double v) { return std::expm1(v); });
    }
}
 
// --- Log1p ---
template <JanusScalar T> T log1p(const T &x) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::log1p(x);
    } else {
        // CasADi: fall back to log(1 + x)
        return log(T(1.0) + x);
    }
}

template <typename Derived> auto log1p(const Eigen::MatrixBase<Derived> &x) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.unaryExpr([](const Scalar &v) { return janus::log1p(v); });
    } else {
        return x.unaryExpr([](double v) { return std::log1p(v); });
    }
}
 
// --- Copysign ---
template <JanusScalar T> T copysign(const T &x, const T &y) {
    if constexpr (std::is_floating_point_v<T>) {
        return std::copysign(x, y);
    } else {
        // CasADi: copysign(x, y) = sign(y) * abs(x)
        return sign(y) * fabs(x);
    }
}

template <JanusScalar T>
    requires(!std::is_same_v<T, double>)
T copysign(const T &x, double y) {
    return janus::copysign(x, T(y));
}

template <JanusScalar T>
    requires(!std::is_same_v<T, double>)
T copysign(double x, const T &y) {
    return janus::copysign(T(x), y);
}

template <typename Derived>
auto copysign(const Eigen::MatrixBase<Derived> &x, const Eigen::MatrixBase<Derived> &y) {
    using Scalar = typename Derived::Scalar;
    if constexpr (std::is_same_v<Scalar, casadi::MX>) {
        return x.binaryExpr(y,
                            [](const Scalar &a, const Scalar &b) { return janus::copysign(a, b); });
    } else {
        return x.binaryExpr(y, [](double a, double b) { return std::copysign(a, b); });
    }
}
 
// --- Square ---
template <JanusScalar T> T square(const T &x) { return x * x; }

template <typename Derived> auto square(const Eigen::MatrixBase<Derived> &x) {
    return x.array().square().matrix();
}
 
} // namespace janus