GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/spatial/spatial-axis.hpp Lines: 40 40 100.0 %
Date: 2024-01-23 21:41:47 Branches: 29 54 53.7 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2017-2019 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_spatial_axis_hpp__
6
#define __pinocchio_spatial_axis_hpp__
7
8
#include "pinocchio/spatial/fwd.hpp"
9
#include "pinocchio/spatial/cartesian-axis.hpp"
10
#include "pinocchio/spatial/motion.hpp"
11
#include "pinocchio/spatial/force.hpp"
12
13
namespace pinocchio
14
{
15
  template<int axis> struct SpatialAxis;
16
17
  template<int axis, typename MotionDerived>
18
  struct MotionAlgebraAction<SpatialAxis<axis>, MotionDerived>
19
  { typedef typename MotionDerived::MotionPlain ReturnType; };
20
21
  template<int _axis>
22
  struct SpatialAxis //: MotionBase< SpatialAxis<_axis> >
23
  {
24
    enum { axis = _axis, dim = 6 };
25
    typedef CartesianAxis<_axis%3> CartesianAxis3;
26
27
    enum { LINEAR = 0, ANGULAR = 3 };
28
29
    template<typename Derived1, typename Derived2>
30
    inline static void cross(const MotionDense<Derived1> & min,
31
                             const MotionDense<Derived2> & mout);
32
33
    template<typename Derived>
34
12
    static typename traits<Derived>::MotionPlain cross(const MotionDense<Derived> & min)
35
    {
36
12
      typename MotionDense<Derived>::MotionPlain res;
37
12
      cross(min,res);
38
12
      return res;
39
    }
40
41
    template<typename Derived1, typename Derived2>
42
    inline static void cross(const ForceDense<Derived1> & fin,
43
                             const ForceDense<Derived2> & fout);
44
45
    template<typename Derived>
46
12
    static typename traits<Derived>::ForcePlain cross(const ForceDense<Derived> & fin)
47
    {
48
12
      typename ForceDense<Derived>::ForcePlain fout;
49
12
      cross(fin,fout);
50
12
      return fout;
51
    }
52
53
    template<typename Scalar>
54
66
    MotionTpl<Scalar> operator*(const Scalar & s) const
55
    {
56
      typedef MotionTpl<Scalar> ReturnType;
57
66
      ReturnType res;
58
462
      for(Eigen::DenseIndex i = 0; i < dim; ++i)
59
396
        res.toVector()[i] = i == axis ? s : Scalar(0);
60
61
66
      return res;
62
    }
63
64
    template<typename Scalar>
65
    friend inline MotionTpl<Scalar>
66
12
    operator*(const Scalar & s, const SpatialAxis &)
67
    {
68
12
      return SpatialAxis() * s;
69
    }
70
71
    template<typename Derived>
72
1068
    friend Derived & operator<<(MotionDense<Derived> & min,
73
                                const SpatialAxis &)
74
    {
75
      typedef typename traits<Derived>::Scalar Scalar;
76
1068
      min.setZero();
77
1068
      min.toVector()[axis] = Scalar(1);
78
1068
      return min.derived();
79
    }
80
81
    template<typename MotionDerived>
82
    typename MotionDerived::MotionPlain
83
110
    motionAction(const MotionDense<MotionDerived> & m) const
84
    {
85
110
      typename MotionDerived::MotionPlain res;
86
      if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
87
      {
88
18
        res.angular().setZero();
89

18
        CartesianAxis3::cross(-m.angular(),res.linear());
90
      }
91
      else
92
      {
93

92
        CartesianAxis3::cross(-m.linear(),res.linear());
94

92
        CartesianAxis3::cross(-m.angular(),res.angular());
95
      }
96
97
110
      return res;
98
    }
99
100
  }; // struct SpatialAxis
101
102
  template<int axis>
103
  template<typename Derived1, typename Derived2>
104
12
  inline void SpatialAxis<axis>::cross(const MotionDense<Derived1> & min,
105
                                       const MotionDense<Derived2> & mout)
106
  {
107
12
    Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,mout);
108
109
    if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
110
    {
111
6
      mout_.angular().setZero();
112

6
      CartesianAxis3::cross(min.angular(),mout_.linear());
113
    }
114
    else
115
    {
116

6
      CartesianAxis3::cross(min.linear(),mout_.linear());
117

6
      CartesianAxis3::cross(min.angular(),mout_.angular());
118
    }
119
12
  }
120
121
  template<int axis>
122
  template<typename Derived1, typename Derived2>
123
24
  inline void SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin,
124
                                       const ForceDense<Derived2> & fout)
125
  {
126
24
    Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,fout);
127
128
    if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
129
    {
130
12
      fout_.linear().setZero();
131

12
      CartesianAxis3::cross(fin.linear(),fout_.angular());
132
    }
133
    else
134
    {
135

12
      CartesianAxis3::cross(fin.linear(),fout_.linear());
136

12
      CartesianAxis3::cross(fin.angular(),fout_.angular());
137
    }
138
24
  }
139
140
  typedef SpatialAxis<0> AxisVX;
141
  typedef SpatialAxis<1> AxisVY;
142
  typedef SpatialAxis<2> AxisVZ;
143
144
  typedef SpatialAxis<3> AxisWX;
145
  typedef SpatialAxis<4> AxisWY;
146
  typedef SpatialAxis<5> AxisWZ;
147
}
148
149
#endif // __pinocchio_spatial_axis_hpp__