delassus-operator-base.hpp

//
// Copyright (c) 2024 INRIA
//
 
#ifndef __pinocchio_algorithm_delassus_operator_base_hpp__
#define __pinocchio_algorithm_delassus_operator_base_hpp__
 
#include "pinocchio/algorithm/fwd.hpp"
#include "pinocchio/math/eigenvalues.hpp"
 
namespace pinocchio
{
 
  template<typename DelassusOperatorDerived>
  struct DelassusOperatorBase
  {
    typedef typename traits<DelassusOperatorDerived>::Scalar Scalar;
    typedef typename traits<DelassusOperatorDerived>::Vector Vector;
    typedef PowerIterationAlgoTpl<Vector> PowerIterationAlgo;
 
    DelassusOperatorDerived & derived()
    {
      return static_cast<DelassusOperatorDerived &>(*this);
    }
    const DelassusOperatorDerived & derived() const
    {
      return static_cast<const DelassusOperatorDerived &>(*this);
    }
 
    explicit DelassusOperatorBase(const Eigen::DenseIndex size)
    : power_iteration_algo(size)
    {
    }
 
    Scalar computeLargestEigenValue(
      const bool reset = true, const int max_it = 10, const Scalar rel_tol = Scalar(1e-8)) const
    {
      power_iteration_algo.max_it = max_it;
      power_iteration_algo.rel_tol = rel_tol;
      if (reset)
        power_iteration_algo.reset();
 
      power_iteration_algo.run(derived());
 
      return power_iteration_algo.largest_eigen_value;
    }
 
    template<typename VectorLike>
    Scalar computeLargestEigenValue(
      const Eigen::PlainObjectBase<VectorLike> & largest_eigenvector_est,
      const bool reset = true,
      const int max_it = 10,
      const Scalar rel_tol = Scalar(1e-8)) const
    {
      PINOCCHIO_CHECK_ARGUMENT_SIZE(largest_eigenvector_est.size(), size());
      power_iteration_algo.max_it = max_it;
      power_iteration_algo.rel_tol = rel_tol;
      if (reset)
        power_iteration_algo.reset();
      power_iteration_algo.principal_eigen_vector = largest_eigenvector_est;
 
      power_iteration_algo.run(derived());
 
      return power_iteration_algo.largest_eigen_value;
    }
 
    Scalar computeLowestEigenValue(
      const bool reset = true,
      const bool compute_largest = true,
      const int max_it = 10,
      const Scalar rel_tol = Scalar(1e-8)) const
    {
      power_iteration_algo.max_it = max_it;
      power_iteration_algo.rel_tol = rel_tol;
      if (reset)
        power_iteration_algo.reset();
 
      power_iteration_algo.lowest(derived(), compute_largest);
 
      return power_iteration_algo.lowest_eigen_value;
    }
 
    template<typename VectorLike1, typename VectorLike2>
    Scalar computeLowestEigenValue(
      const Eigen::PlainObjectBase<VectorLike1> & largest_eigenvector_est,
      const Eigen::PlainObjectBase<VectorLike2> & lowest_eigenvector_est,
      const bool reset = true,
      const bool compute_largest = true,
      const int max_it = 10,
      const Scalar rel_tol = Scalar(1e-8)) const
    {
      PINOCCHIO_CHECK_ARGUMENT_SIZE(largest_eigenvector_est.size(), size());
      PINOCCHIO_CHECK_ARGUMENT_SIZE(lowest_eigenvector_est.size(), size());
 
      power_iteration_algo.max_it = max_it;
      power_iteration_algo.rel_tol = rel_tol;
      if (reset)
        power_iteration_algo.reset();
      power_iteration_algo.principal_eigen_vector = largest_eigenvector_est;
      power_iteration_algo.lowest_eigen_vector = lowest_eigenvector_est;
 
      power_iteration_algo.lowest(derived(), compute_largest);
 
      return power_iteration_algo.lowest_eigen_value;
    }
 
    template<typename VectorLike>
    void updateDamping(const Eigen::MatrixBase<VectorLike> & vec)
    {
      derived().updateDamping(vec.derived());
    }
 
    void updateDamping(const Scalar mu)
    {
      derived().updateDamping(mu);
    }
 
    template<typename MatrixLike>
    void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const
    {
      derived().solveInPlace(mat.const_cast_derived());
    }
 
    template<typename MatrixLike>
    typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike)
      solve(const Eigen::MatrixBase<MatrixLike> & mat) const
    {
      return derived().solve(mat);
    }
 
    template<typename MatrixDerivedIn, typename MatrixDerivedOut>
    void solve(
      const Eigen::MatrixBase<MatrixDerivedIn> & x,
      const Eigen::MatrixBase<MatrixDerivedOut> & res) const
    {
      derived().solve(x.derived(), res.const_cast_derived());
    }
 
    template<typename MatrixIn, typename MatrixOut>
    void applyOnTheRight(
      const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res) const
    {
      derived().applyOnTheRight(x.derived(), res.const_cast_derived());
    }
 
    template<typename MatrixDerived>
    typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
    operator*(const Eigen::MatrixBase<MatrixDerived> & x) const
    {
      return derived() * x.derived();
    }
 
    Eigen::DenseIndex size() const
    {
      return derived().size();
    }
    Eigen::DenseIndex rows() const
    {
      return derived().rows();
    }
    Eigen::DenseIndex cols() const
    {
      return derived().cols();
    }
 
    PowerIterationAlgo & getPowerIterationAlgo()
    {
      return power_iteration_algo;
    }
 
    const PowerIterationAlgo & getPowerIterationAlgo() const
    {
      return power_iteration_algo;
    }
 
  protected:
    mutable PowerIterationAlgo power_iteration_algo;
 
  }; // struct DelassusOperatorBase
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_algorithm_delassus_operator_base_hpp__