<commit_before>//
// Copyright (c) 2015 CNRS
//
// 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_hpp__
#define __se3_force_hpp__

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"


namespace se3
{

  template< class Derived>
  class ForceBase
  {
  protected:

    typedef Derived  Derived_t;
    SPATIAL_TYPEDEF_TEMPLATE(Derived_t);

  public:
    Derived_t & derived() { return *static_cast<Derived_t*>(this); }
    const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }

    const Angular_t & angular() const  { return derived().angular_impl(); }
    const Linear_t & linear() const  { return derived().linear_impl(); }
    Angular_t & angular()  { return derived().angular_impl(); }
    Linear_t & linear()   { return derived().linear_impl(); }
    void angular(const Angular_t & R) { derived().angular_impl(R); }
    void linear(const Linear_t & R) { derived().linear_impl(R); }


    Vector6 toVector() const
    {
      return derived().toVector_impl();
    }
    operator Vector6 () const { return toVector(); }


    void disp(std::ostream & os) const
    {
      static_cast<const Derived_t*>(this)->disp_impl(os);
    }

    Derived_t & operator= (const Derived_t & other) { return derived().__equl__(other); }
    Derived_t& operator+= (const Derived_t & phi) { return derived().__pequ__(phi); }
    Derived_t operator+(const Derived_t & phi) const { return derived().__plus__(phi); }
    Derived_t operator*(double a) const    { return derived().__mult__(a); }
    Derived_t operator-() const { return derived().__minus__(); }
    Derived_t operator-(const Derived_t & phi) const { return derived().__minus__(phi); }


    /// af = aXb.act(bf)
    Derived_t se3Action(const SE3 & m) const
    {
      return derived().se3Action_impl(m);
    }

    Derived_t se3ActionInverse(const SE3 & m) const
    {
      return derived().se3ActionInverse_impl(m);
    }

    friend std::ostream & operator << (std::ostream & os,const ForceBase<Derived> & X)
    { 
      X.disp(os);
      return os;
    }

  }; // class ForceBase


  template<typename T, int U>
  struct traits< ForceTpl<T, U> >
  {
    typedef T Scalar_t;
    typedef Eigen::Matrix<T,3,1,U> Vector3;
    typedef Eigen::Matrix<T,4,1,U> Vector4;
    typedef Eigen::Matrix<T,6,1,U> Vector6;
    typedef Eigen::Matrix<T,3,3,U> Matrix3;
    typedef Eigen::Matrix<T,4,4,U> Matrix4;
    typedef Eigen::Matrix<T,6,6,U> Matrix6;
    typedef Vector3 Angular_t;
    typedef Vector3 Linear_t;
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<T,U> Quaternion_t;
    typedef SE3Tpl<T,U> SE3;
    typedef ForceTpl<T,U> Force;
    typedef MotionTpl<T,U> Motion;
    typedef Symmetric3Tpl<T,U> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits ForceTpl

  template<typename _Scalar, int _Options>
  class ForceTpl : public ForceBase< ForceTpl< _Scalar, _Options > >
  {

  public:
    friend class ForceBase< ForceTpl< _Scalar, _Options > >;
    SPATIAL_TYPEDEF_TEMPLATE(ForceTpl);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW


    ForceTpl() : m_n(), m_f() {}


    template<typename f3_t,typename n3_t>
    ForceTpl(const Eigen::MatrixBase<f3_t> & f,const Eigen::MatrixBase<n3_t> & n)
    : m_n(n),
    m_f(f)
    {

    }

    template<typename f6>
    explicit ForceTpl(const Eigen::MatrixBase<f6> & f)
    : m_n(f.template segment<3>(ANGULAR)),
    m_f(f.template segment<3>(LINEAR)) 
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(f6);
      assert( f.size() == 6 );
    }


    template<typename S2,int O2>
    explicit ForceTpl(const ForceTpl<S2,O2> & clone)
    : m_n(clone.angular()),
    m_f(clone.linear())
    {

    }


    static ForceTpl Zero() { return ForceTpl(Linear_t::Zero(), Angular_t::Zero()); }
    static ForceTpl Random() { return ForceTpl(Linear_t::Random(), Angular_t::Random()); }


    ForceTpl & setZero () { m_n.setZero (); m_f.setZero (); return *this; } 
    ForceTpl & setRandom () { m_n.setRandom (); m_f.setRandom (); return *this; }


    Vector6 toVector_impl() const
    {
      Vector6 f;
      f.template segment<3>(ANGULAR) = m_n;
      f.template segment<3>(LINEAR)  = m_f;
      return f;
    }



    void disp_impl(std::ostream & os) const
    {
      os
      << "f =\n" << m_f << std::endl
      << "tau =\n" << m_n << std::endl;
    }

    /// af = aXb.act(bf)
    ForceTpl se3Action_impl(const SE3 & m) const
    {
      Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) );
      return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl());
    }


    ForceTpl se3ActionInverse_impl(const SE3 & m) const
    {
      return ForceTpl(m.rotation().transpose()*linear_impl(),
        m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) );
    }<commit_msg>[C++] Change the display of force class - similar to motion class<commit_after>//
// Copyright (c) 2015 CNRS
//
// 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_hpp__
#define __se3_force_hpp__

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"


namespace se3
{

  template< class Derived>
  class ForceBase
  {
  protected:

    typedef Derived  Derived_t;
    SPATIAL_TYPEDEF_TEMPLATE(Derived_t);

  public:
    Derived_t & derived() { return *static_cast<Derived_t*>(this); }
    const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }

    const Angular_t & angular() const  { return derived().angular_impl(); }
    const Linear_t & linear() const  { return derived().linear_impl(); }
    Angular_t & angular()  { return derived().angular_impl(); }
    Linear_t & linear()   { return derived().linear_impl(); }
    void angular(const Angular_t & R) { derived().angular_impl(R); }
    void linear(const Linear_t & R) { derived().linear_impl(R); }


    Vector6 toVector() const
    {
      return derived().toVector_impl();
    }
    operator Vector6 () const { return toVector(); }


    void disp(std::ostream & os) const
    {
      static_cast<const Derived_t*>(this)->disp_impl(os);
    }

    Derived_t & operator= (const Derived_t & other) { return derived().__equl__(other); }
    Derived_t& operator+= (const Derived_t & phi) { return derived().__pequ__(phi); }
    Derived_t operator+(const Derived_t & phi) const { return derived().__plus__(phi); }
    Derived_t operator*(double a) const    { return derived().__mult__(a); }
    Derived_t operator-() const { return derived().__minus__(); }
    Derived_t operator-(const Derived_t & phi) const { return derived().__minus__(phi); }


    /// af = aXb.act(bf)
    Derived_t se3Action(const SE3 & m) const
    {
      return derived().se3Action_impl(m);
    }

    Derived_t se3ActionInverse(const SE3 & m) const
    {
      return derived().se3ActionInverse_impl(m);
    }

    friend std::ostream & operator << (std::ostream & os,const ForceBase<Derived> & X)
    { 
      X.disp(os);
      return os;
    }

  }; // class ForceBase


  template<typename T, int U>
  struct traits< ForceTpl<T, U> >
  {
    typedef T Scalar_t;
    typedef Eigen::Matrix<T,3,1,U> Vector3;
    typedef Eigen::Matrix<T,4,1,U> Vector4;
    typedef Eigen::Matrix<T,6,1,U> Vector6;
    typedef Eigen::Matrix<T,3,3,U> Matrix3;
    typedef Eigen::Matrix<T,4,4,U> Matrix4;
    typedef Eigen::Matrix<T,6,6,U> Matrix6;
    typedef Vector3 Angular_t;
    typedef Vector3 Linear_t;
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<T,U> Quaternion_t;
    typedef SE3Tpl<T,U> SE3;
    typedef ForceTpl<T,U> Force;
    typedef MotionTpl<T,U> Motion;
    typedef Symmetric3Tpl<T,U> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits ForceTpl

  template<typename _Scalar, int _Options>
  class ForceTpl : public ForceBase< ForceTpl< _Scalar, _Options > >
  {

  public:
    friend class ForceBase< ForceTpl< _Scalar, _Options > >;
    SPATIAL_TYPEDEF_TEMPLATE(ForceTpl);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW


    ForceTpl() : m_n(), m_f() {}


    template<typename f3_t,typename n3_t>
    ForceTpl(const Eigen::MatrixBase<f3_t> & f,const Eigen::MatrixBase<n3_t> & n)
    : m_n(n),
    m_f(f)
    {

    }

    template<typename f6>
    explicit ForceTpl(const Eigen::MatrixBase<f6> & f)
    : m_n(f.template segment<3>(ANGULAR)),
    m_f(f.template segment<3>(LINEAR)) 
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(f6);
      assert( f.size() == 6 );
    }


    template<typename S2,int O2>
    explicit ForceTpl(const ForceTpl<S2,O2> & clone)
    : m_n(clone.angular()),
    m_f(clone.linear())
    {

    }


    static ForceTpl Zero() { return ForceTpl(Linear_t::Zero(), Angular_t::Zero()); }
    static ForceTpl Random() { return ForceTpl(Linear_t::Random(), Angular_t::Random()); }


    ForceTpl & setZero () { m_n.setZero (); m_f.setZero (); return *this; } 
    ForceTpl & setRandom () { m_n.setRandom (); m_f.setRandom (); return *this; }


    Vector6 toVector_impl() const
    {
      Vector6 f;
      f.template segment<3>(ANGULAR) = m_n;
      f.template segment<3>(LINEAR)  = m_f;
      return f;
    }



    void disp_impl(std::ostream & os) const
    {
      os
      << "f = " << linear_impl().transpose() << std::endl
      << "tau = " << angular_impl().transpose() << std::endl;
    }

    /// af = aXb.act(bf)
    ForceTpl se3Action_impl(const SE3 & m) const
    {
      Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) );
      return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl());
    }


    ForceTpl se3ActionInverse_impl(const SE3 & m) const
    {
      return ForceTpl(m.rotation().transpose()*linear_impl(),
        m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) );
    }
{'fraction_non_alphanumeric': 0.13512535071047155, 'fraction_numerical': 0.01212779437053127, 'mean_word_length': 3.324853228962818, 'pattern_counts': {'":': 0, '<': 109, '<?xml version=': 0, '>': 83, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5142192', 'n_tokens_mistral': 3878, 'n_tokens_neox': 3701, 'n_words': 900}