force-tpl.hpp

//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
 
#ifndef __pinocchio_spatial_force_tpl_hpp__
#define __pinocchio_spatial_force_tpl_hpp__
 
namespace pinocchio
{
  template<typename _Scalar, int _Options>
  struct traits<ForceTpl<_Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
    typedef Eigen::Matrix<Scalar, 6, 1, _Options> Vector6;
    typedef Eigen::Matrix<Scalar, 6, 6, _Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
    typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
    typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef ForceTpl<Scalar, _Options> ForcePlain;
    enum
    {
      LINEAR = 0,
      ANGULAR = 3,
      Options = _Options
    };
 
    typedef ForceRef<Vector6> ForceRefType;
  }; // traits ForceTpl
 
  template<typename _Scalar, int _Options>
  class ForceTpl : public ForceDense<ForceTpl<_Scalar, _Options>>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ForceDense<ForceTpl> Base;
    FORCE_TYPEDEF_TPL(ForceTpl);
    enum
    {
      Options = _Options
    };
 
    using Base::operator=;
    using Base::operator!=;
    using Base::angular;
    using Base::linear;
 
    // Constructors
    ForceTpl()
    {
    }
 
    template<typename V1, typename V2>
    ForceTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1);
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2);
      linear() = v;
      angular() = w;
    }
 
    template<typename V6>
    explicit ForceTpl(const Eigen::MatrixBase<V6> & v)
    : m_data(v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
    }
 
    ForceTpl(const ForceTpl & clone) // Copy constructor
    : m_data(clone.toVector())
    {
    }
 
    template<typename S2, int O2>
    explicit ForceTpl(const ForceTpl<S2, O2> & other)
    {
      *this = other.template cast<Scalar>();
    }
 
    template<typename F2>
    explicit ForceTpl(const ForceBase<F2> & clone)
    {
      *this = clone;
    }
 
    ForceTpl & operator=(const ForceTpl & clone) // Copy assignment operator
    {
      m_data = clone.toVector();
      return *this;
    }
 
    template<int O2>
    explicit ForceTpl(const ForceTpl<Scalar, O2> & clone)
    : m_data(clone.toVector())
    {
    }
 
    template<typename M2>
    explicit ForceTpl(const ForceDense<M2> & clone)
    {
      linear() = clone.linear();
      angular() = clone.angular();
    }
 
    // initializers
    static ForceTpl Zero()
    {
      return ForceTpl(Vector6::Zero());
    }
    static ForceTpl Random()
    {
      return ForceTpl(Vector6::Random());
    }
 
    ToVectorConstReturnType toVector_impl() const
    {
      return m_data;
    }
    ToVectorReturnType toVector_impl()
    {
      return m_data;
    }
 
    // Getters
    ConstAngularType angular_impl() const
    {
      return m_data.template segment<3>(ANGULAR);
    }
    ConstLinearType linear_impl() const
    {
      return m_data.template segment<3>(LINEAR);
    }
    AngularType angular_impl()
    {
      return m_data.template segment<3>(ANGULAR);
    }
    LinearType linear_impl()
    {
      return m_data.template segment<3>(LINEAR);
    }
 
    template<typename V3>
    void angular_impl(const Eigen::MatrixBase<V3> & w)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
      angular_impl() = w;
    }
    template<typename V3>
    void linear_impl(const Eigen::MatrixBase<V3> & v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
      linear_impl() = v;
    }
 
    ForceRef<Vector6> ref()
    {
      return ForceRef<Vector6>(m_data);
    }
 
    template<typename NewScalar>
    ForceTpl<NewScalar, Options> cast() const
    {
      typedef ForceTpl<NewScalar, Options> ReturnType;
      ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>());
      return res;
    }
 
  protected:
    Vector6 m_data;
 
  }; // class ForceTpl
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_spatial_force_tpl_hpp__