force-base.hpp

//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
 
#ifndef __pinocchio_spatial_force_base_hpp__
#define __pinocchio_spatial_force_base_hpp__
 
namespace pinocchio
{
  template<class Derived>
  class ForceBase : NumericalBase<Derived>
  {
  public:
    FORCE_TYPEDEF_TPL(Derived);
 
    Derived & derived()
    {
      return *static_cast<Derived *>(this);
    }
    const Derived & derived() const
    {
      return *static_cast<const Derived *>(this);
    }
 
    Derived & const_cast_derived() const
    {
      return *const_cast<Derived *>(&derived());
    }
 
    ConstAngularType angular() const
    {
      return derived().angular_impl();
    }
 
    ConstLinearType linear() const
    {
      return derived().linear_impl();
    }
 
    AngularType angular()
    {
      return derived().angular_impl();
    }
 
    LinearType linear()
    {
      return derived().linear_impl();
    }
 
    template<typename V3Like>
    void angular(const Eigen::MatrixBase<V3Like> & n)
    {
      derived().angular_impl(n.derived());
    }
 
    template<typename V3Like>
    void linear(const Eigen::MatrixBase<V3Like> & f)
    {
      derived().linear_impl(f.derived());
    }
 
    ToVectorConstReturnType toVector() const
    {
      return derived().toVector_impl();
    }
 
    ToVectorReturnType toVector()
    {
      return derived().toVector_impl();
    }
 
    /*
     * @brief C-style cast operator
     * \copydoc ForceBase::toVector
     */
    operator Vector6() const
    {
      return toVector();
    }
 
    template<typename F2>
    bool operator==(const ForceBase<F2> & other) const
    {
      return derived().isEqual_impl(other.derived());
    }
 
    template<typename F2>
    bool operator!=(const ForceBase<F2> & other) const
    {
      return !(derived() == other.derived());
    }
 
    bool isApprox(
      const Derived & other,
      const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return derived().isApprox_impl(other, prec);
    }
 
    bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return derived().isZero_impl(prec);
    }
 
    Derived & operator=(const ForceBase<Derived> & other)
    {
      return derived().setFrom(other.derived());
    }
 
    Derived & operator+=(const ForceBase<Derived> & phi)
    {
      return derived().__pequ__(phi.derived());
    }
 
    Derived & operator-=(const ForceBase<Derived> & phi)
    {
      return derived().__mequ__(phi.derived());
    }
 
    Derived operator+(const ForceBase<Derived> & phi) const
    {
      return derived().__plus__(phi.derived());
    }
 
    template<typename OtherScalar>
    ForcePlain operator*(const OtherScalar & alpha) const
    {
      return derived().__mult__(alpha);
    }
 
    template<typename OtherScalar>
    ForcePlain operator/(const OtherScalar & alpha) const
    {
      return derived().__div__(alpha);
    }
 
    Derived operator-() const
    {
      return derived().__opposite__();
    }
 
    Derived operator-(const ForceBase<Derived> & phi) const
    {
      return derived().__minus__(phi.derived());
    }
 
    template<typename MotionDerived>
    Scalar dot(const MotionDense<MotionDerived> & m) const
    {
      return derived().dot(m.derived());
    }
 
    template<typename S2, int O2>
    typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<S2, O2> & m) const
    {
      return derived().se3Action_impl(m);
    }
 
    template<typename S2, int O2>
    typename SE3GroupAction<Derived>::ReturnType se3ActionInverse(const SE3Tpl<S2, O2> & m) const
    {
      return derived().se3ActionInverse_impl(m);
    }
 
    template<typename M1>
    typename MotionAlgebraAction<Derived, M1>::ReturnType
    motionAction(const MotionDense<M1> & v) const
    {
      return derived().motionAction(v.derived());
    }
 
    void disp(std::ostream & os) const
    {
      derived().disp_impl(os);
    }
    friend std::ostream & operator<<(std::ostream & os, const ForceBase<Derived> & X)
    {
      X.disp(os);
      return os;
    }
 
  }; // class ForceBase
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_spatial_force_base_hpp__