pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
10namespace 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 segment(
24 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 segment(
34 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 middleCols(
51 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 middleCols(
61 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 middleRows(
78 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 middleRows(
88 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 block(
105 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 block(
118 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 segment(
142 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 segment(
151 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 middleCols(
167 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 middleCols(
176 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 middleRows(
192 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 middleRows(
201 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 block(
217 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 block(
228 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} // namespace pinocchio
238
239#endif // ifndef __pinocchio_math_matrix_block_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::Dynamic
const int Dynamic
Definition fwd.hpp:140