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