force-base.hpp

//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_force_base_hpp__
#define __se3_force_base_hpp__

namespace se3
{
  template< class Derived>
  class ForceBase
  {
  public:
    FORCE_TYPEDEF_TPL(Derived);
    
    Derived & derived() { return *static_cast<Derived*>(this); }
    const Derived& derived() const { return *static_cast<const Derived*>(this); }
    
    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); }
    
    Derived & operator= (const Derived & other) { return derived().__equl__(other); }
    
    Derived & operator+= (const Derived & phi) { return derived().__pequ__(phi); }
    
    Derived & operator-= (const Derived & phi) { return derived().__mequ__(phi); }
    
    Derived operator+(const Derived & phi) const { return derived().__plus__(phi); }
    
    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 Derived & phi) const { return derived().__minus__(phi); }
    
    Scalar dot(const Motion & m) const { return derived().dot(m); }
    
    
    template<typename S2, int O2>
    typename internal::SE3GroupAction<Derived>::ReturnType
    se3Action(const SE3Tpl<S2,O2> & m) const
    { return derived().se3Action_impl(m); }
    
    template<typename S2, int O2>
    typename internal::SE3GroupAction<Derived>::ReturnType
    se3ActionInverse(const SE3Tpl<S2,O2> & m) const
    { return derived().se3ActionInverse_impl(m); }
    
    template<typename M1>
    typename internal::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 se3

#endif // ifndef __se3_force_base_hpp__