tangent-bundle.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/core/manifold-base.hpp"
 
namespace aligator {

template <class Base>
struct TangentBundleTpl : public ManifoldAbstractTpl<typename Base::Scalar> {
protected:
  Base base_;
 
public:
  using Self = TangentBundleTpl<Base>;
  using Scalar = typename Base::Scalar;
  static constexpr int Options = Base::Options;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);

  TangentBundleTpl(Base base)
      : base_(base) {};

  template <typename... BaseCtorArgs>
  TangentBundleTpl(BaseCtorArgs... args)
      : base_(Base(args...)) {}

 
  inline int nx() const { return base_.nx() + base_.ndx(); }
  inline int ndx() const { return 2 * base_.ndx(); }
 
  bool isNormalized(const ConstVectorRef &x) const {
    auto p = getBasePoint(x);
    return base_.isNormalized(p);
  }
 
  const Base &getBaseSpace() const { return base_; }

  template <typename Point>
  typename Point::ConstSegmentReturnType
  getBasePoint(const Eigen::MatrixBase<Point> &x) const {
    return x.head(base_.nx());
  }
 
  template <typename Point>
  typename Point::SegmentReturnType
  getBasePointWrite(const Eigen::MatrixBase<Point> &x) const {
    return x.const_cast_derived().head(base_.nx());
  }
 
  template <typename Tangent>
  typename Tangent::ConstSegmentReturnType
  getBaseTangent(const Tangent &v) const {
    return v.head(base_.ndx());
  }
 
  template <typename Tangent>
  typename Tangent::SegmentReturnType
  getTangentHeadWrite(const Eigen::MatrixBase<Tangent> &v) const {
    return v.const_cast_derived().head(base_.ndx());
  }
 
  template <typename Jac>
  Eigen::Block<Jac, Eigen::Dynamic, Eigen::Dynamic>
  getBaseJacobian(const Eigen::MatrixBase<Jac> &J) const {
    return J.const_cast_derived().topLeftCorner(base_.ndx(), base_.ndx());
  }
 
protected:
 
  void neutral_impl(VectorRef out) const;
  void rand_impl(VectorRef out) const;
 
  void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &dx,
                      VectorRef out) const;
 
  void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
                       VectorRef out) const;
 
  void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
                       MatrixRef Jout, int arg) const;
 
  void JintegrateTransport_impl(const ConstVectorRef &x,
                                const ConstVectorRef &v, MatrixRef Jout,
                                int arg) const;
 
  void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
                        MatrixRef Jout, int arg) const;
 
  void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
                        const Scalar &u, VectorRef out) const;
};
 
} // namespace aligator
 
#include "aligator/modelling/spaces/tangent-bundle.hxx"