JanusTypes.hpp

Go to the documentation of this file.

#pragma once
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <casadi/casadi.hpp>
#include <limits>
 
namespace Eigen {
template <> struct NumTraits<casadi::MX> : GenericNumTraits<casadi::MX> {
    enum {
        IsComplex = 0,
        IsInteger = 0,
        IsSigned = 1,
        RequireInitialization = 1,
        ReadCost = 1,
        AddCost = 1,
        MulCost = 1
    };
 
    static inline int digits10() { return std::numeric_limits<double>::digits10; }
    static inline int min_exponent() { return std::numeric_limits<double>::min_exponent; }
    static inline int max_exponent() { return std::numeric_limits<double>::max_exponent; }
    static inline casadi::MX epsilon() { return std::numeric_limits<double>::epsilon(); }
    static inline casadi::MX dummy_precision() { return 1e-5; }
    static inline casadi::MX highest() { return std::numeric_limits<double>::max(); }
    static inline casadi::MX lowest() { return std::numeric_limits<double>::lowest(); }
};
} // namespace Eigen
 
namespace janus {
 
// --- Matrix Types ---
template <typename Scalar>
using JanusMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

template <typename Scalar> using JanusVector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
 
// --- Fixed-Size Types ---
template <typename Scalar> using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
template <typename Scalar> using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
template <typename Scalar> using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
 
template <typename Scalar> using Mat2 = Eigen::Matrix<Scalar, 2, 2>;
template <typename Scalar> using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
template <typename Scalar> using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
 
// Numeric Backend
using NumericScalar = double;                     
using NumericMatrix = JanusMatrix<NumericScalar>; 
using NumericVector = JanusVector<NumericScalar>; 
 
// Symbolic Backend
using SymbolicScalar = casadi::MX;                  
using SymbolicMatrix = JanusMatrix<SymbolicScalar>; 
using SymbolicVector = JanusVector<SymbolicScalar>; 
 
// --- Sparse Numeric Types ---
using SparseMatrix = Eigen::SparseMatrix<double>; 
using SparseTriplet = Eigen::Triplet<double>;     
 
// --- Symbolic Variable Creation ---

inline SymbolicScalar sym(const std::string &name) { return casadi::MX::sym(name); }

inline SymbolicScalar sym(const std::string &name, int rows, int cols = 1) {
    return casadi::MX::sym(name, rows, cols);
}

inline SymbolicVector sym_vector(const std::string &name, int size) {
    casadi::MX mx = casadi::MX::sym(name, size, 1);
    SymbolicVector v(size);
    for (int i = 0; i < size; ++i) {
        v(i) = mx(i);
    }
    return v;
}

inline SymbolicVector sym_vec(const std::string &name, int size) {
    // Delegate to sym_vector - they are equivalent
    return sym_vector(name, size);
}

inline std::pair<SymbolicVector, SymbolicScalar> sym_vec_pair(const std::string &name, int size) {
    casadi::MX mx = casadi::MX::sym(name, size, 1);
    SymbolicVector v(size);
    for (int i = 0; i < size; ++i) {
        v(i) = mx(i);
    }
    return {v, mx};
}

inline SymbolicScalar as_mx(const SymbolicVector &v) {
    casadi::MX m(v.size(), 1);
    for (int i = 0; i < v.size(); ++i) {
        m(i) = v(i);
    }
    return m;
}
 
// --- Universal Conversion Helpers ---

template <typename Derived> casadi::MX to_mx(const Eigen::MatrixBase<Derived> &e) {
    if (e.size() == 0)
        return casadi::MX(e.rows(), e.cols());
 
    // Create an MX of correct shape
    casadi::MX m(e.rows(), e.cols());
    // Fill it element-wise
    for (Eigen::Index i = 0; i < e.rows(); ++i) {
        for (Eigen::Index j = 0; j < e.cols(); ++j) {
            if constexpr (std::is_same_v<typename Derived::Scalar, casadi::MX>) {
                m(static_cast<int>(i), static_cast<int>(j)) = e(i, j);
            } else {
                m(static_cast<int>(i), static_cast<int>(j)) = casadi::MX(e(i, j));
            }
        }
    }
    return m;
}

inline Eigen::Matrix<casadi::MX, Eigen::Dynamic, Eigen::Dynamic> to_eigen(const casadi::MX &m) {
    Eigen::Matrix<casadi::MX, Eigen::Dynamic, Eigen::Dynamic> e(m.size1(), m.size2());
    for (int i = 0; i < m.size1(); ++i) {
        for (int j = 0; j < m.size2(); ++j) {
            e(i, j) = m(i, j);
        }
    }
    return e;
}

inline SymbolicVector as_vector(const casadi::MX &m) {
    SymbolicVector v(m.size1());
    for (int i = 0; i < m.size1(); ++i) {
        v(i) = m(i);
    }
    return v;
}

inline SymbolicVector to_eigen_vec(const casadi::MX &m) { return as_vector(m); }

class SymbolicArg {
  public:
    SymbolicArg(const SymbolicScalar &s) : mx_(s) {}

    template <typename Derived> SymbolicArg(const Eigen::MatrixBase<Derived> &e) {
        if (e.size() == 0) {
            mx_ = casadi::MX(e.rows(), e.cols());
            return;
        }
        mx_ = casadi::MX(e.rows(), e.cols());
        for (Eigen::Index i = 0; i < e.rows(); ++i) {
            for (Eigen::Index j = 0; j < e.cols(); ++j) {
                mx_(static_cast<int>(i), static_cast<int>(j)) = e(i, j);
            }
        }
    }

    operator SymbolicScalar() const { return mx_; }

    SymbolicScalar get() const { return mx_; }
 
  private:
    SymbolicScalar mx_;
};
 
} // namespace janus