Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/force-tpl.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 31 | 47 | 66.0% |
Branches: | 3 | 30 | 10.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2019 CNRS INRIA | ||
3 | // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
4 | // | ||
5 | |||
6 | #ifndef __pinocchio_spatial_force_tpl_hpp__ | ||
7 | #define __pinocchio_spatial_force_tpl_hpp__ | ||
8 | |||
9 | namespace pinocchio | ||
10 | { | ||
11 | template<typename _Scalar, int _Options> | ||
12 | struct traits<ForceTpl<_Scalar, _Options>> | ||
13 | { | ||
14 | typedef _Scalar Scalar; | ||
15 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
16 | typedef Eigen::Matrix<Scalar, 6, 1, _Options> Vector6; | ||
17 | typedef Eigen::Matrix<Scalar, 6, 6, _Options> Matrix6; | ||
18 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; | ||
19 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; | ||
20 | typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType; | ||
21 | typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType; | ||
22 | typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType; | ||
23 | typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType; | ||
24 | typedef ForceTpl<Scalar, _Options> ForcePlain; | ||
25 | enum | ||
26 | { | ||
27 | LINEAR = 0, | ||
28 | ANGULAR = 3, | ||
29 | Options = _Options | ||
30 | }; | ||
31 | |||
32 | typedef ForceRef<Vector6> ForceRefType; | ||
33 | }; // traits ForceTpl | ||
34 | |||
35 | template<typename _Scalar, int _Options> | ||
36 | class ForceTpl : public ForceDense<ForceTpl<_Scalar, _Options>> | ||
37 | { | ||
38 | public: | ||
39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
40 | typedef ForceDense<ForceTpl> Base; | ||
41 | FORCE_TYPEDEF_TPL(ForceTpl); | ||
42 | enum | ||
43 | { | ||
44 | Options = _Options | ||
45 | }; | ||
46 | |||
47 | using Base::operator=; | ||
48 | using Base::operator!=; | ||
49 | using Base::angular; | ||
50 | using Base::linear; | ||
51 | |||
52 | // Constructors | ||
53 | 158552 | ForceTpl() | |
54 | 158552 | { | |
55 | 158552 | } | |
56 | |||
57 | template<typename V1, typename V2> | ||
58 | 54344 | ForceTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w) | |
59 | 54344 | { | |
60 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1); | ||
61 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2); | ||
62 |
1/2✓ Branch 2 taken 52193 times.
✗ Branch 3 not taken.
|
54344 | linear() = v; |
63 |
1/2✓ Branch 2 taken 52193 times.
✗ Branch 3 not taken.
|
54344 | angular() = w; |
64 | 54344 | } | |
65 | |||
66 | template<typename V6> | ||
67 | 4968 | explicit ForceTpl(const Eigen::MatrixBase<V6> & v) | |
68 | 4968 | : m_data(v) | |
69 | { | ||
70 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); | ||
71 | 4968 | } | |
72 | |||
73 | 53441 | ForceTpl(const ForceTpl & clone) // Copy constructor | |
74 | 53441 | : m_data(clone.toVector()) | |
75 | { | ||
76 | 53441 | } | |
77 | |||
78 | template<typename S2, int O2> | ||
79 | ✗ | explicit ForceTpl(const ForceTpl<S2, O2> & other) | |
80 | { | ||
81 | ✗ | *this = other.template cast<Scalar>(); | |
82 | } | ||
83 | |||
84 | template<typename F2> | ||
85 | explicit ForceTpl(const ForceBase<F2> & clone) | ||
86 | { | ||
87 | *this = clone; | ||
88 | } | ||
89 | |||
90 | 101196 | ForceTpl & operator=(const ForceTpl & clone) // Copy assignment operator | |
91 | { | ||
92 | 101196 | m_data = clone.toVector(); | |
93 | 101196 | return *this; | |
94 | } | ||
95 | |||
96 | template<int O2> | ||
97 | explicit ForceTpl(const ForceTpl<Scalar, O2> & clone) | ||
98 | : m_data(clone.toVector()) | ||
99 | { | ||
100 | } | ||
101 | |||
102 | template<typename M2> | ||
103 | ✗ | explicit ForceTpl(const ForceDense<M2> & clone) | |
104 | { | ||
105 | ✗ | linear() = clone.linear(); | |
106 | ✗ | angular() = clone.angular(); | |
107 | } | ||
108 | |||
109 | // initializers | ||
110 | 4457 | static ForceTpl Zero() | |
111 | { | ||
112 |
1/2✓ Branch 2 taken 4457 times.
✗ Branch 3 not taken.
|
4457 | return ForceTpl(Vector6::Zero()); |
113 | } | ||
114 | ✗ | static ForceTpl Random() | |
115 | { | ||
116 | ✗ | return ForceTpl(Vector6::Random()); | |
117 | } | ||
118 | |||
119 | 136516 | ToVectorConstReturnType toVector_impl() const | |
120 | { | ||
121 | 136516 | return m_data; | |
122 | } | ||
123 | 40990 | ToVectorReturnType toVector_impl() | |
124 | { | ||
125 | 40990 | return m_data; | |
126 | } | ||
127 | |||
128 | // Getters | ||
129 | 220447 | ConstAngularType angular_impl() const | |
130 | { | ||
131 | 220447 | return m_data.template segment<3>(ANGULAR); | |
132 | } | ||
133 | 262814 | ConstLinearType linear_impl() const | |
134 | { | ||
135 | 262814 | return m_data.template segment<3>(LINEAR); | |
136 | } | ||
137 | 303962 | AngularType angular_impl() | |
138 | { | ||
139 | 303962 | return m_data.template segment<3>(ANGULAR); | |
140 | } | ||
141 | 302303 | LinearType linear_impl() | |
142 | { | ||
143 | 302303 | return m_data.template segment<3>(LINEAR); | |
144 | } | ||
145 | |||
146 | template<typename V3> | ||
147 | ✗ | void angular_impl(const Eigen::MatrixBase<V3> & w) | |
148 | { | ||
149 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
150 | ✗ | angular_impl() = w; | |
151 | } | ||
152 | template<typename V3> | ||
153 | ✗ | void linear_impl(const Eigen::MatrixBase<V3> & v) | |
154 | { | ||
155 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
156 | ✗ | linear_impl() = v; | |
157 | } | ||
158 | |||
159 | ✗ | ForceRef<Vector6> ref() | |
160 | { | ||
161 | ✗ | return ForceRef<Vector6>(m_data); | |
162 | } | ||
163 | |||
164 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
165 | template<typename NewScalar> | ||
166 | ✗ | ForceTpl<NewScalar, Options> cast() const | |
167 | { | ||
168 | typedef ForceTpl<NewScalar, Options> ReturnType; | ||
169 | ✗ | ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>()); | |
170 | ✗ | return res; | |
171 | } | ||
172 | |||
173 | protected: | ||
174 | Vector6 m_data; | ||
175 | |||
176 | }; // class ForceTpl | ||
177 | |||
178 | } // namespace pinocchio | ||
179 | |||
180 | #endif // ifndef __pinocchio_spatial_force_tpl_hpp__ | ||
181 |