pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
tensor.hpp
1//
2// Copyright (c) 2019-2020 INRIA
3//
4
5#ifndef __pinocchio_math_tensor_hpp__
6#define __pinocchio_math_tensor_hpp__
7
8#include "pinocchio/fwd.hpp"
9
10#if !EIGEN_VERSION_AT_LEAST(3, 2, 90)
11 #define EIGEN_DEVICE_FUNC
12#endif
13
14#ifndef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
15 #if (__cplusplus <= 199711L && EIGEN_COMP_MSVC < 1900) || defined(__CUDACC__) \
16 || defined(EIGEN_AVOID_STL_ARRAY)
17namespace Eigen
18{
19 template<typename T, std::size_t n>
20 struct array
21 {
22 EIGEN_DEVICE_FUNC
23 EIGEN_STRONG_INLINE T & operator[](size_t index)
24 {
25 return values[index];
26 }
27 EIGEN_DEVICE_FUNC
28 EIGEN_STRONG_INLINE const T & operator[](size_t index) const
29 {
30 return values[index];
31 }
32
33 EIGEN_DEVICE_FUNC
34 EIGEN_STRONG_INLINE T & front()
35 {
36 return values[0];
37 }
38 EIGEN_DEVICE_FUNC
39 EIGEN_STRONG_INLINE const T & front() const
40 {
41 return values[0];
42 }
43
44 EIGEN_DEVICE_FUNC
45 EIGEN_STRONG_INLINE T & back()
46 {
47 return values[n - 1];
48 }
49 EIGEN_DEVICE_FUNC
50 EIGEN_STRONG_INLINE const T & back() const
51 {
52 return values[n - 1];
53 }
54
55 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static std::size_t size()
56 {
57 return n;
58 }
59
60 T values[n];
61 };
62
63 template<class T, std::size_t n>
64 EIGEN_DEVICE_FUNC bool operator==(const array<T, n> & lhs, const array<T, n> & rhs)
65 {
66 for (std::size_t i = 0; i < n; ++i)
67 {
68 if (lhs[i] != rhs[i])
69 {
70 return false;
71 }
72 }
73 return true;
74 }
75
76 template<class T, std::size_t n>
77 EIGEN_DEVICE_FUNC bool operator!=(const array<T, n> & lhs, const array<T, n> & rhs)
78 {
79 return !(lhs == rhs);
80 }
81} // namespace Eigen
82 #else
83 #include <array>
84namespace Eigen
85{
86 template<typename T, std::size_t N>
87 using array = std::array<T, N>;
88} // namespace Eigen
89 #endif
90#endif
91
92namespace pinocchio
93{
94
95#ifndef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
96
97 // Mimic the Eigen::Tensor module only available for C++11 and more
98 template<
99 typename Scalar_,
100 int NumIndices_,
101 int Options_ = 0,
102 typename IndexType = Eigen::DenseIndex>
103 struct Tensor
104 {
105 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
106
107 typedef Scalar_ Scalar;
108 enum
109 {
110 Options = Options_,
111 NumIndices = NumIndices_
112 };
113 typedef IndexType Index;
114 typedef Eigen::array<Index, NumIndices_> Dimensions;
115
116 inline Tensor & base()
117 {
118 return *this;
119 }
120 inline const Tensor & base() const
121 {
122 return *this;
123 }
124
125 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Dimensions & dimensions()
126 {
127 return m_dimensions;
128 }
129 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions & dimensions() const
130 {
131 return m_dimensions;
132 }
133
134 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rank() const
135 {
136 return NumIndices;
137 }
138
139 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index dimension(std::size_t n) const
140 {
141 assert(n <= NumIndices && "n is larger than the dimension of the tensor.");
142 return m_dimensions[n];
143 }
144
145 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const
146 {
147 return m_storage.size();
148 }
149
150 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
151 {
152 return m_storage.data();
153 }
154
155 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
156 {
157 return m_storage.data();
158 }
159
160 EIGEN_DEVICE_FUNC
161 EIGEN_STRONG_INLINE Tensor & setZero()
162 {
163 return setConstant(Scalar(0));
164 }
165
166 EIGEN_DEVICE_FUNC
167 EIGEN_STRONG_INLINE Tensor & setConstant(const Scalar & val)
168 {
169 m_storage.setConstant(val);
170 return *this;
171 }
172
173 EIGEN_DEVICE_FUNC
174 EIGEN_STRONG_INLINE Tensor & setRandom()
175 {
176 m_storage.setRandom();
177 return *this;
178 }
179
180 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor()
181 : m_storage()
182 {
183 }
184
185 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(const Tensor & other)
186 : m_storage(other.m_storage)
187 , m_dimensions(other.m_dimensions)
188 {
189 }
190
191 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Tensor(Index dim1)
192 : m_storage(dim1)
193 {
194 m_dimensions[0] = dim1;
195 EIGEN_STATIC_ASSERT(1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
196 }
197
198 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2)
199 : m_storage(dim1 * dim2)
200 {
201 m_dimensions[0] = dim1;
202 m_dimensions[1] = dim2;
203 EIGEN_STATIC_ASSERT(2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
204 }
205
206 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3)
207 : m_storage(dim1 * dim2 * dim3)
208 {
209 m_dimensions[0] = dim1;
210 m_dimensions[1] = dim2;
211 m_dimensions[2] = dim3;
212 EIGEN_STATIC_ASSERT(3 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
213 }
214
215 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3, Index dim4)
216 : m_storage(dim1 * dim2 * dim3 * dim4)
217 {
218 m_dimensions[0] = dim1;
219 m_dimensions[1] = dim2;
220 m_dimensions[2] = dim3;
221 m_dimensions[3] = dim4;
222 EIGEN_STATIC_ASSERT(4 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
223 }
224
225 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
226 Tensor(Index dim1, Index dim2, Index dim3, Index dim4, Index dim5)
227 : m_storage(dim1 * dim2 * dim3 * dim4 * dim5)
228 {
229 m_dimensions[0] = dim1;
230 m_dimensions[1] = dim2;
231 m_dimensions[2] = dim3;
232 m_dimensions[3] = dim4;
233 m_dimensions[4] = dim5;
234 EIGEN_STATIC_ASSERT(5 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
235 }
236
237 EIGEN_DEVICE_FUNC
238 EIGEN_STRONG_INLINE const Scalar & operator()(Index i0) const
239 {
240 EIGEN_STATIC_ASSERT(1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
241 return m_storage.coeff(i0);
242 }
243
244 EIGEN_DEVICE_FUNC
245 EIGEN_STRONG_INLINE const Scalar & operator()(Index i0, Index i1) const
246 {
247 EIGEN_STATIC_ASSERT(2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
248 return m_storage.coeff(i0 + i1 * m_dimensions[0]);
249 }
250
251 EIGEN_DEVICE_FUNC
252 EIGEN_STRONG_INLINE const Scalar & operator()(Index i0, Index i1, Index i2) const
253 {
254 EIGEN_STATIC_ASSERT(3 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
255 return m_storage.coeff(i0 + i1 * m_dimensions[0] + i2 * m_dimensions[1] * m_dimensions[0]);
256 }
257
258 EIGEN_DEVICE_FUNC
259 EIGEN_STRONG_INLINE const Scalar & operator()(Index i0, Index i1, Index i2, Index i3) const
260 {
261 EIGEN_STATIC_ASSERT(4 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
262 return coeff(
263 i0 + i1 * m_dimensions[0] + i2 * m_dimensions[1] * m_dimensions[0]
264 + i3 * m_dimensions[2] * m_dimensions[1] * m_dimensions[0]);
265 }
266
267 EIGEN_DEVICE_FUNC
268 EIGEN_STRONG_INLINE const Scalar &
269 operator()(Index i0, Index i1, Index i2, Index i3, Index i4) const
270 {
271 EIGEN_STATIC_ASSERT(5 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
272 return coeff(
273 i0 + i1 * m_dimensions[0] + i2 * m_dimensions[1] * m_dimensions[0]
274 + i3 * m_dimensions[2] * m_dimensions[1] * m_dimensions[0]
275 + i4 * m_dimensions[3] * m_dimensions[2] * m_dimensions[1] * m_dimensions[0]);
276 }
277
278 EIGEN_DEVICE_FUNC
279 void resize(const Eigen::array<Index, NumIndices> & dimensions)
280 {
281 size_t i;
282 Index size = Index(1);
283 for (i = 0; i < NumIndices; i++)
284 {
285 Eigen::internal::check_rows_cols_for_overflow<Eigen::Dynamic>::run(size, dimensions[i]);
286 size *= dimensions[i];
287 }
288
289 for (i = 0; i < NumIndices; i++)
290 m_dimensions[i] = dimensions[i];
291
292 bool size_changed = size != this->size();
293 if (size_changed)
294 m_storage.resize(size);
295
296 #ifdef EIGEN_INITIALIZE_COEFFS
297 if (size_changed)
298 {
299 #if defined(EIGEN_INITIALIZE_MATRICES_BY_ZERO)
300 m_storage.fill(Scalar(0));
301 #elif defined(EIGEN_INITIALIZE_MATRICES_BY_NAN)
302 m_storage.fill(std::numeric_limits<Scalar>::quiet_NaN());
303 #endif
304 }
305 #endif
306 }
307
308 EIGEN_DEVICE_FUNC bool operator==(const Tensor & other) const
309 {
310 return m_storage == other.m_storage;
311 }
312
313 EIGEN_DEVICE_FUNC bool operator!=(const Tensor & other) const
314 {
315 return m_storage != other.m_storage;
316 }
317
318 protected:
319 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> StorageType;
320 StorageType m_storage;
321
322 Dimensions m_dimensions;
323 };
324
325#else
326
327 // Use the default Eigen::Tensor module
328 template<
329 typename Scalar_,
330 int NumIndices_,
331 int Options_ = 0,
332 typename IndexType = Eigen::DenseIndex>
333 using Tensor = Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>;
334
335#endif // ifndef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
336
337} // namespace pinocchio
338
339#if !EIGEN_VERSION_AT_LEAST(3, 2, 90)
340 #undef EIGEN_DEVICE_FUNC
341#endif
342
343#endif // ifndef __pinocchio_math_tensor_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11