GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/spatial-axis.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 14 38 36.8%
Branches: 4 54 7.4%
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>
16 struct SpatialAxis;
17
18 template<int axis, typename MotionDerived>
19 struct MotionAlgebraAction<SpatialAxis<axis>, MotionDerived>
20 {
21 typedef typename MotionDerived::MotionPlain ReturnType;
22 };
23
24 template<int _axis>
25 struct SpatialAxis //: MotionBase< SpatialAxis<_axis> >
26 {
27 enum
28 {
29 axis = _axis,
30 dim = 6
31 };
32 typedef CartesianAxis<_axis % 3> CartesianAxis3;
33
34 enum
35 {
36 LINEAR = 0,
37 ANGULAR = 3
38 };
39
40 template<typename Derived1, typename Derived2>
41 inline static void cross(const MotionDense<Derived1> & min, const MotionDense<Derived2> & mout);
42
43 template<typename Derived>
44 ✗ static typename traits<Derived>::MotionPlain cross(const MotionDense<Derived> & min)
45 {
46 ✗ typename MotionDense<Derived>::MotionPlain res;
47 ✗ cross(min, res);
48 ✗ return res;
49 }
50
51 template<typename Derived1, typename Derived2>
52 inline static void cross(const ForceDense<Derived1> & fin, const ForceDense<Derived2> & fout);
53
54 template<typename Derived>
55 ✗ static typename traits<Derived>::ForcePlain cross(const ForceDense<Derived> & fin)
56 {
57 ✗ typename ForceDense<Derived>::ForcePlain fout;
58 ✗ cross(fin, fout);
59 ✗ return fout;
60 }
61
62 template<typename Scalar>
63 112 MotionTpl<Scalar> operator*(const Scalar & s) const
64 {
65 typedef MotionTpl<Scalar> ReturnType;
66 112 ReturnType res;
67
2/2
✓ Branch 0 taken 336 times.
✓ Branch 1 taken 56 times.
 784 for (Eigen::DenseIndex i = 0; i < dim; ++i)
68
2/2
✓ Branch 0 taken 56 times.
✓ Branch 1 taken 280 times.
 672 res.toVector()[i] = i == axis ? s : Scalar(0);
69
70 112 return res;
71 }
72
73 template<typename Scalar>
74 ✗ friend inline MotionTpl<Scalar> operator*(const Scalar & s, const SpatialAxis &)
75 {
76 ✗ return SpatialAxis() * s;
77 }
78
79 template<typename Derived>
80 27 friend Derived & operator<<(MotionDense<Derived> & min, const SpatialAxis &)
81 {
82 typedef typename traits<Derived>::Scalar Scalar;
83 27 min.setZero();
84 27 min.toVector()[axis] = Scalar(1);
85 27 return min.derived();
86 }
87
88 template<typename MotionDerived>
89 56 typename MotionDerived::MotionPlain motionAction(const MotionDense<MotionDerived> & m) const
90 {
91 56 typename MotionDerived::MotionPlain res;
92 if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3))
93 {
94 ✗ res.angular().setZero();
95 ✗ CartesianAxis3::cross(-m.angular(), res.linear());
96 }
97 else
98 {
99
0/6
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 56 CartesianAxis3::cross(-m.linear(), res.linear());
100
0/6
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 56 CartesianAxis3::cross(-m.angular(), res.angular());
101 }
102
103 56 return res;
104 }
105
106 }; // struct SpatialAxis
107
108 template<int axis>
109 template<typename Derived1, typename Derived2>
110 inline void
111 ✗ SpatialAxis<axis>::cross(const MotionDense<Derived1> & min, const MotionDense<Derived2> & mout)
112 {
113 ✗ Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2, mout);
114
115 if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3))
116 {
117 ✗ mout_.angular().setZero();
118 ✗ CartesianAxis3::cross(min.angular(), mout_.linear());
119 }
120 else
121 {
122 ✗ CartesianAxis3::cross(min.linear(), mout_.linear());
123 ✗ CartesianAxis3::cross(min.angular(), mout_.angular());
124 }
125 }
126
127 template<int axis>
128 template<typename Derived1, typename Derived2>
129 inline void
130 ✗ SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin, const ForceDense<Derived2> & fout)
131 {
132 ✗ Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2, fout);
133
134 if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3))
135 {
136 ✗ fout_.linear().setZero();
137 ✗ CartesianAxis3::cross(fin.linear(), fout_.angular());
138 }
139 else
140 {
141 ✗ CartesianAxis3::cross(fin.linear(), fout_.linear());
142 ✗ CartesianAxis3::cross(fin.angular(), fout_.angular());
143 }
144 }
145
146 typedef SpatialAxis<0> AxisVX;
147 typedef SpatialAxis<1> AxisVY;
148 typedef SpatialAxis<2> AxisVZ;
149
150 typedef SpatialAxis<3> AxisWX;
151 typedef SpatialAxis<4> AxisWY;
152 typedef SpatialAxis<5> AxisWZ;
153 } // namespace pinocchio
154
155 #endif // __pinocchio_spatial_axis_hpp__
156