pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
matrix-block.hpp
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #ifndef __pinocchio_math_matrix_block_hpp__
6 #define __pinocchio_math_matrix_block_hpp__
7 
8 #include <Eigen/Core>
9 
10 namespace pinocchio
11 {
12  template<int NV>
13  struct SizeDepType
14  {
15  template<class Mat>
16  struct SegmentReturn
17  {
18  typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
19  typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
20  };
21 
22  template<typename D>
23  static typename SegmentReturn<D>::ConstType
24  segment(const Eigen::MatrixBase<D> & mat,
25  typename Eigen::DenseBase<D>::Index start,
26  typename Eigen::DenseBase<D>::Index size = NV)
27  {
28  PINOCCHIO_UNUSED_VARIABLE(size);
29  return mat.template segment<NV>(start);
30  }
31 
32  template<typename D>
33  static typename SegmentReturn<D>::Type
34  segment(Eigen::MatrixBase<D> & mat,
35  typename Eigen::DenseBase<D>::Index start,
36  typename Eigen::DenseBase<D>::Index size = NV)
37  {
38  PINOCCHIO_UNUSED_VARIABLE(size);
39  return mat.template segment<NV>(start);
40  }
41 
42  template<class Mat>
43  struct ColsReturn
44  {
45  typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
46  typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
47  };
48 
49  template<typename D>
50  static typename ColsReturn<D>::ConstType
51  middleCols(const Eigen::MatrixBase<D> & mat,
52  typename Eigen::DenseBase<D>::Index start,
53  typename Eigen::DenseBase<D>::Index size = NV)
54  {
55  PINOCCHIO_UNUSED_VARIABLE(size);
56  return mat.template middleCols<NV>(start);
57  }
58 
59  template<typename D>
60  static typename ColsReturn<D>::Type
61  middleCols(Eigen::MatrixBase<D> & mat,
62  typename Eigen::DenseBase<D>::Index start,
63  typename Eigen::DenseBase<D>::Index size = NV)
64  {
65  PINOCCHIO_UNUSED_VARIABLE(size);
66  return mat.template middleCols<NV>(start);
67  }
68 
69  template<class Mat>
70  struct RowsReturn
71  {
72  typedef typename Mat::template NRowsBlockXpr<NV>::Type Type;
73  typedef typename Mat::template ConstNRowsBlockXpr<NV>::Type ConstType;
74  };
75 
76  template<typename D>
77  static typename RowsReturn<D>::ConstType
78  middleRows(const Eigen::MatrixBase<D> & mat,
79  typename Eigen::DenseBase<D>::Index start,
80  typename Eigen::DenseBase<D>::Index size = NV)
81  {
82  PINOCCHIO_UNUSED_VARIABLE(size);
83  return mat.template middleRows<NV>(start);
84  }
85 
86  template<typename D>
87  static typename RowsReturn<D>::Type
88  middleRows(Eigen::MatrixBase<D> & mat,
89  typename Eigen::DenseBase<D>::Index start,
90  typename Eigen::DenseBase<D>::Index size = NV)
91  {
92  PINOCCHIO_UNUSED_VARIABLE(size);
93  return mat.template middleRows<NV>(start);
94  }
95 
96  template<class Mat>
97  struct BlockReturn
98  {
99  typedef Eigen::Block<Mat, NV, NV> Type;
100  typedef const Eigen::Block<const Mat, NV, NV> ConstType;
101  };
102 
103  template<typename D>
104  static typename BlockReturn<D>::ConstType
105  block(const Eigen::MatrixBase<D> & mat,
106  typename Eigen::DenseBase<D>::Index row_id,
107  typename Eigen::DenseBase<D>::Index col_id,
108  typename Eigen::DenseBase<D>::Index row_size_block = NV,
109  typename Eigen::DenseBase<D>::Index col_size_block = NV)
110  {
111  PINOCCHIO_UNUSED_VARIABLE(row_size_block);
112  PINOCCHIO_UNUSED_VARIABLE(col_size_block);
113  return mat.template block<NV,NV>(row_id,col_id);
114  }
115 
116  template<typename D>
117  static typename BlockReturn<D>::Type
118  block(Eigen::MatrixBase<D> & mat,
119  typename Eigen::DenseBase<D>::Index row_id,
120  typename Eigen::DenseBase<D>::Index col_id,
121  typename Eigen::DenseBase<D>::Index row_size_block = NV,
122  typename Eigen::DenseBase<D>::Index col_size_block = NV)
123  {
124  PINOCCHIO_UNUSED_VARIABLE(row_size_block);
125  PINOCCHIO_UNUSED_VARIABLE(col_size_block);
126  return mat.template block<NV,NV>(row_id,col_id);
127  }
128  };
129 
130  template<>
131  struct SizeDepType<Eigen::Dynamic>
132  {
133  template<class Mat>
134  struct SegmentReturn
135  {
136  typedef typename Mat::SegmentReturnType Type;
137  typedef typename Mat::ConstSegmentReturnType ConstType;
138  };
139 
140  template<typename D>
141  static typename SegmentReturn<D>::ConstType
142  segment(const Eigen::MatrixBase<D> & mat,
143  typename Eigen::DenseBase<D>::Index start,
144  typename Eigen::DenseBase<D>::Index size)
145  {
146  return mat.segment(start,size);
147  }
148 
149  template<typename D>
150  static typename SegmentReturn<D>::Type
151  segment(Eigen::MatrixBase<D> & mat,
152  typename Eigen::DenseBase<D>::Index start,
153  typename Eigen::DenseBase<D>::Index size)
154  {
155  return mat.segment(start,size);
156  }
157 
158  template<class Mat>
159  struct ColsReturn
160  {
161  typedef typename Mat::ColsBlockXpr Type;
162  typedef typename Mat::ConstColsBlockXpr ConstType;
163  };
164 
165  template<typename D>
166  static typename ColsReturn<D>::ConstType
167  middleCols(const Eigen::MatrixBase<D> & mat,
168  typename Eigen::DenseBase<D>::Index start,
169  typename Eigen::DenseBase<D>::Index size)
170  {
171  return mat.middleCols(start,size);
172  }
173 
174  template<typename D>
175  static typename ColsReturn<D>::Type
176  middleCols(Eigen::MatrixBase<D> & mat,
177  typename Eigen::DenseBase<D>::Index start,
178  typename Eigen::DenseBase<D>::Index size)
179  {
180  return mat.middleCols(start,size);
181  }
182 
183  template<class Mat>
184  struct RowsReturn
185  {
186  typedef typename Mat::RowsBlockXpr Type;
187  typedef typename Mat::ConstRowsBlockXpr ConstType;
188  };
189 
190  template<typename D>
191  static typename RowsReturn<D>::ConstType
192  middleRows(const Eigen::MatrixBase<D> & mat,
193  typename Eigen::DenseBase<D>::Index start,
194  typename Eigen::DenseBase<D>::Index size)
195  {
196  return mat.middleRows(start,size);
197  }
198 
199  template<typename D>
200  static typename RowsReturn<D>::Type
201  middleRows(Eigen::MatrixBase<D> & mat,
202  typename Eigen::DenseBase<D>::Index start,
203  typename Eigen::DenseBase<D>::Index size)
204  {
205  return mat.middleRows(start,size);
206  }
207 
208  template<class Mat>
209  struct BlockReturn
210  {
211  typedef Eigen::Block<Mat> Type;
212  typedef const Eigen::Block<const Mat> ConstType;
213  };
214 
215  template<typename D>
216  static typename BlockReturn<D>::ConstType
217  block(const Eigen::MatrixBase<D> & mat,
218  typename Eigen::DenseBase<D>::Index row_id,
219  typename Eigen::DenseBase<D>::Index col_id,
220  typename Eigen::DenseBase<D>::Index row_size_block,
221  typename Eigen::DenseBase<D>::Index col_size_block)
222  {
223  return mat.block(row_id,col_id,row_size_block,col_size_block);
224  }
225 
226  template<typename D>
227  static typename BlockReturn<D>::Type
228  block(Eigen::MatrixBase<D> & mat,
229  typename Eigen::DenseBase<D>::Index row_id,
230  typename Eigen::DenseBase<D>::Index col_id,
231  typename Eigen::DenseBase<D>::Index row_size_block,
232  typename Eigen::DenseBase<D>::Index col_size_block)
233  {
234  return mat.block(row_id,col_id,row_size_block,col_size_block);
235  }
236  };
237 }
238 
239 #endif // ifndef __pinocchio_math_matrix_block_hpp__
Eigen
Definition: casadi.hpp:47
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24