EigenVector.h

#pragma once
 
#include <Caribou/config.h>
#include <Caribou/macros.h>
#include <Caribou/traits.h>
#include <sofa/defaulttype/BaseMatrix.h>
 
#include <Eigen/Dense>
 
namespace SofaCaribou::Algebra {
 
template <typename Derived, typename Enable = void>
class EigenVector : public sofa::defaulttype::BaseVector
{
    static_assert(
            std::is_base_of_v<Eigen::EigenBase<std::decay_t<Derived> >, std::decay_t<Derived> >,
            "The class template argument 'Derived' must be derived from Eigen::EigenBase<Derived>."
    );
 
    static_assert(
        std::remove_reference_t<Derived>::IsVectorAtCompileTime,
        "The class template argument 'Derived' must be an Eigen vector (either 1 or dynamic column"
        " at compile time, or 1 or dynamic row at compile time."
    );
 
public:
    using EigenType = std::remove_cv_t<std::remove_reference_t<Derived>>;
    using Base = sofa::defaulttype::BaseVector;
    using Index = Base::Index;
    using Real = typename EigenType::Scalar;
 
    explicit EigenVector(std::remove_reference_t<Derived> & eigen_vector) : p_eigen_vector(eigen_vector) {}
 
    EigenVector(Eigen::Index n) : p_eigen_vector(n) {}
 
    [[nodiscard]]
    Index size() const final {return static_cast<int>(p_eigen_vector.size());}
 
    [[nodiscard]]
    SReal element(Index index) const final {return static_cast<SReal>(p_eigen_vector[index]);}
 
    void resize(Index n) final {p_eigen_vector.resize(n);p_eigen_vector.setZero();}
 
    void clear() final {p_eigen_vector.setZero();}
 
    void set(Index index, SReal value) final {p_eigen_vector[index] = static_cast<Real>(value);}
 
    void add(Index index, SReal value) final {p_eigen_vector[index] += static_cast<Real>(value);}
 
    Base::ElementType getElementType() const final {
        return (std::is_integral_v<Real> ? Base::ELEMENT_INT : Base::ELEMENT_FLOAT);
    }
 
    std::size_t getElementSize() const final {
        return sizeof(Real);
    }
 
    const EigenType & vector() const {return p_eigen_vector;}
 
    EigenType & vector() {return p_eigen_vector;}
 
private:
    Derived p_eigen_vector;
};
 
} // namespace SofaCaribou::Algebra