pinocchio  3.3.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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)
17 namespace 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>
84 namespace 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 
92 namespace 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