manifold-base.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/fwd.hpp"
 
namespace aligator {

template <typename _Scalar> struct ManifoldAbstractTpl {
public:
  using Scalar = _Scalar; 
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using TangentSpaceType = VectorSpaceTpl<Scalar, Eigen::Dynamic>;
 
  ManifoldAbstractTpl(int nx, int ndx)
      : nx_(nx)
      , ndx_(ndx) {}
 
  virtual ~ManifoldAbstractTpl() = default;

  /// @brief    Get manifold representation dimension.
  inline int nx() const { return nx_; }
  inline int ndx() const { return ndx_; }

  /// @brief Get the neutral element \f$e \in M\f$ from the manifold (if this
  /// makes sense).
  [[nodiscard]] VectorXs neutral() const {
    VectorXs out(nx());
    neutral_impl(out);
    return out;
  }

  void neutral(VectorRef out) const { neutral_impl(out); }

  [[nodiscard]] VectorXs rand() const {
    VectorXs out(nx());
    rand_impl(out);
    return out;
  }

  void rand(VectorRef out) const { rand_impl(out); }

  virtual bool isNormalized(const ConstVectorRef & /*x*/) const { return true; }

  TangentSpaceType tangentSpace() const {
    return TangentSpaceType(this->ndx());
  }


  void integrate(const ConstVectorRef &x, const ConstVectorRef &v,
                 VectorRef out) const;

  void Jintegrate(const ConstVectorRef &x, const ConstVectorRef &v,
                  MatrixRef Jout, int arg) const;

  void JintegrateTransport(const ConstVectorRef &x, const ConstVectorRef &v,
                           MatrixRef Jout, int arg) const {
    assert(Jout.rows() == v.size());
    JintegrateTransport_impl(x, v, Jout, arg);
  }

  void difference(const ConstVectorRef &x0, const ConstVectorRef &x1,
                  VectorRef out) const;

  void Jdifference(const ConstVectorRef &x0, const ConstVectorRef &x1,
                   MatrixRef Jout, int arg) const;
 
  void interpolate(const ConstVectorRef &x0, const ConstVectorRef &x1,
                   const Scalar &u, VectorRef out) const;


  [[nodiscard]] VectorXs integrate(const ConstVectorRef &x,
                                   const ConstVectorRef &v) const {
    VectorXs out(nx());
    integrate_impl(x, v, out);
    return out;
  }

  [[nodiscard]] VectorXs difference(const ConstVectorRef &x0,
                                    const ConstVectorRef &x1) const {
    VectorXs out(ndx());
    difference_impl(x0, x1, out);
    return out;
  }

  [[nodiscard]] VectorXs interpolate(const ConstVectorRef &x0,
                                     const ConstVectorRef &x1,
                                     const Scalar &u) const {
    VectorXs out(nx());
    interpolate_impl(x0, x1, u, out);
    return out;
  }

 
protected:
  int nx_;
  int ndx_;

  virtual void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
                              VectorRef out) const = 0;
 
  virtual void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
                               MatrixRef Jout, int arg) const = 0;
 
  virtual void JintegrateTransport_impl(const ConstVectorRef &x,
                                        const ConstVectorRef &v, MatrixRef Jout,
                                        int arg) const = 0;

  virtual void difference_impl(const ConstVectorRef &x0,
                               const ConstVectorRef &x1,
                               VectorRef out) const = 0;
 
  virtual void Jdifference_impl(const ConstVectorRef &x0,
                                const ConstVectorRef &x1, MatrixRef Jout,
                                int arg) const = 0;

  virtual void interpolate_impl(const ConstVectorRef &x0,
                                const ConstVectorRef &x1, const Scalar &u,
                                VectorRef out) const {
    // default implementation
    integrate(x0, u * difference(x0, x1), out);
  }
 
  virtual void neutral_impl(VectorRef out) const {
    assert(out.size() == nx());
    out.setZero();
  }
 
  virtual void rand_impl(VectorRef out) const {
    assert(out.size() == nx());
    out.setRandom();
  }
};
 
} // namespace aligator
 
#include "aligator/core/manifold-base.hxx"