Lodestar
An integrated real-time control package in C++
DemuxBlock.hpp
1 //
2 // Created by Hamza El-Kebir on 12/23/21.
3 //
4 
5 #ifndef LODESTAR_DEMUXBLOCK_HPP
6 #define LODESTAR_DEMUXBLOCK_HPP
7 
8 #include "Lodestar/blocks/Block.hpp"
9 #include "Lodestar/aux/TemplateTools.hpp"
10 #include "Lodestar/aux/CompileTimeQualifiers.hpp"
11 #include "Eigen/Dense"
12 
13 namespace ls {
14  namespace blocks {
15  namespace std {
16  enum class DemuxBlockOperator {
17  RowMajor,
18  ColMajor
19  };
20 
21  template<typename TType, DemuxBlockOperator TOps = DemuxBlockOperator::RowMajor>
22  class DemuxBlock :
23  public Block<
24  ::std::tuple<TType>,
25  ::std::tuple<TType>,
26  ::std::tuple<DemuxBlockOperator>
27  > {
28  static_assert(true,
29  "DemuxBlock not defined for this type.");
30  };
31 
32  // TODO: Look into support for dynamically size matrices.
33  template<typename TScalar, int TRows, int TCols, DemuxBlockOperator TOps>
34  class DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps> :
35  public Block<
36  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
37  typename aux::TemplateTools::repeat<TScalar,
38  TRows * TCols, ::std::tuple>::type,
39  ::std::tuple<DemuxBlockOperator>
40  > {
41  public:
42  using Base =
43  Block<
44  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
45  typename aux::TemplateTools::repeat<TScalar,
46  TRows * TCols, ::std::tuple>::type,
47  ::std::tuple<DemuxBlockOperator>
48  >;
49 
50  DemuxBlock()
51  {
52  setOperator(TOps);
53  bindEquation();
54  }
55 
56  explicit DemuxBlock(const DemuxBlockOperator ops)
57  {
58  setOperator(ops);
59  bindEquation();
60  }
61 
62  void setOperator(const DemuxBlockOperator ops)
63  {
64  this->template p<0>() = ops;
65  }
66 
67  DemuxBlockOperator getOperator() const
68  {
69  return this->template p<0>();
70  }
71 
72  template<int TTRows = TRows, int TTCols = TCols,
73  typename ::std::enable_if<
74  ((TTRows * TTCols > 0) &&
75  (TTRows == 1 ||
76  TTCols == 1)), void *>::type * = nullptr>
77  Signal<TScalar> &x()
78  {
79  return this->template i<0>();
80  }
81 
82  template<int TTRows = TRows, int TTCols = TCols,
83  typename ::std::enable_if<
84  !((TTRows * TTCols > 0) &&
85  (TTRows == 1 ||
86  TTCols == 1)), void *>::type * = nullptr>
87  Signal<TScalar> &x()
88  {
89  static_assert(
90  (TTRows * TTCols > 0) &&
91  (TTRows == 1 || TTCols == 1),
92  "Demux contains less than 1 output and/or does not map to a 1D object.");
93  return this->template i<0>();
94  }
95 
96  template<int TTRows = TRows, int TTCols = TCols,
97  typename ::std::enable_if<
98  (TTRows * TTCols > 0) &&
99  (TTRows == 1 ||
100  TTCols == 1), void *>::type * = nullptr>
101  const Signal<TScalar> &x() const
102  {
103  return this->template i<0>();
104  }
105 
106  template<int TTRows = TRows, int TTCols = TCols,
107  typename ::std::enable_if<
108  !((TTRows * TTCols > 0) &&
109  (TTRows == 1 ||
110  TTCols == 1)), void *>::type * = nullptr>
111  const Signal<TScalar> &x() const
112  {
113  static_assert(
114  (TTRows * TTCols > 0) &&
115  (TTRows == 1 || TTCols == 1),
116  "Demux contains less than 1 output and/or does not map to a 1D object.");
117  return this->template i<0>();
118  }
119 
120  template<int TTRows = TRows, int TTCols = TCols,
121  typename ::std::enable_if<
122  (TTRows * TTCols > 1) &&
123  (TTRows == 1 ||
124  TTCols == 1), void *>::type * = nullptr>
125  Signal<TScalar> &y()
126  {
127  return this->template i<1>();
128  }
129 
130  template<int TTRows = TRows, int TTCols = TCols,
131  typename ::std::enable_if<
132  !((TTRows * TTCols > 1) &&
133  (TTRows == 1 ||
134  TTCols == 1)), void *>::type * = nullptr>
135  Signal<TScalar> &y()
136  {
137  static_assert(
138  (TTRows * TTCols > 1) &&
139  (TTRows == 1 || TTCols == 1),
140  "Demux contains less than 2 outputs and/or does not map to a 1D object.");
141  return this->template i<1>();
142  }
143 
144  template<int TTRows = TRows, int TTCols = TCols,
145  typename ::std::enable_if<
146  (TTRows * TTCols > 1) &&
147  (TTRows == 1 ||
148  TTCols == 1), void *>::type * = nullptr>
149  const Signal<TScalar> &y() const
150  {
151  return this->template i<1>();
152  }
153 
154  template<int TTRows = TRows, int TTCols = TCols,
155  typename ::std::enable_if<
156  !((TTRows * TTCols > 1) &&
157  (TTRows == 1 ||
158  TTCols == 1)), void *>::type * = nullptr>
159  const Signal<TScalar> &y() const
160  {
161  static_assert(
162  (TTRows * TTCols > 1) &&
163  (TTRows == 1 || TTCols == 1),
164  "Demux contains less than 2 outputs and/or does not map to a 1D object.");
165  return this->template i<1>();
166  }
167 
168  template<int TTRows = TRows, int TTCols = TCols,
169  typename ::std::enable_if<
170  (TTRows * TTCols > 2) &&
171  (TTRows == 1 ||
172  TTCols == 1), void *>::type * = nullptr>
173  Signal<TScalar> &z()
174  {
175  return this->template o<2>();
176  }
177 
178  template<int TTRows = TRows, int TTCols = TCols,
179  typename ::std::enable_if<
180  !((TTRows * TTCols > 2) &&
181  (TTRows == 1 ||
182  TTCols == 1)), void *>::type * = nullptr>
183  Signal<TScalar> &z()
184  {
185  static_assert(
186  (TTRows * TTCols > 2) &&
187  (TTRows == 1 || TTCols == 1),
188  "Demux contains less than 3 outputs and/or does not map to a 1D object.");
189  return this->template o<2>();
190  }
191 
192  template<int TTRows = TRows, int TTCols = TCols,
193  typename ::std::enable_if<
194  (TTRows * TTCols > 2) &&
195  (TTRows == 1 ||
196  TTCols == 1), void *>::type * = nullptr>
197  const Signal<TScalar> &z() const
198  {
199  return this->template o<2>();
200  }
201 
202  template<int TTRows = TRows, int TTCols = TCols,
203  typename ::std::enable_if<
204  !((TTRows * TTCols > 2) &&
205  (TTRows == 1 ||
206  TTCols == 1)), void *>::type * = nullptr>
207  const Signal<TScalar> &z() const
208  {
209  static_assert(
210  (TTRows * TTCols > 2) &&
211  (TTRows == 1 || TTCols == 1),
212  "Demux contains less than 3 outputs and/or does not map to a 1D object.");
213  return this->template o<2>();
214  }
215 
216  template<int TTRows = TRows, int TTCols = TCols,
217  typename ::std::enable_if<
218  (TTRows * TTCols > 3) &&
219  (TTRows == 1 ||
220  TTCols == 1), void *>::type * = nullptr>
221  Signal<TScalar> &w()
222  {
223  return this->template o<3>();
224  }
225 
226  template<int TTRows = TRows, int TTCols = TCols,
227  typename ::std::enable_if<
228  !((TTRows * TTCols > 3) &&
229  (TTRows == 1 ||
230  TTCols == 1)), void *>::type * = nullptr>
231  Signal<TScalar> &w()
232  {
233  static_assert(
234  (TTRows * TTCols > 3) &&
235  (TTRows == 1 || TTCols == 1),
236  "Demux contains less than 4 outputs and/or does not map to a 1D object.");
237  return this->template o<3>();
238  }
239 
240  template<int TTRows = TRows, int TTCols = TCols,
241  typename ::std::enable_if<
242  (TTRows * TTCols > 3) &&
243  (TTRows == 1 ||
244  TTCols == 1), void *>::type * = nullptr>
245  const Signal<TScalar> &w() const
246  {
247  return this->template o<3>();
248  }
249 
250  template<int TTRows = TRows, int TTCols = TCols,
251  typename ::std::enable_if<
252  !((TTRows * TTCols > 3) &&
253  (TTRows == 1 ||
254  TTCols == 1)), void *>::type * = nullptr>
255  const Signal<TScalar> &w() const
256  {
257  static_assert(
258  (TTRows * TTCols > 3) &&
259  (TTRows == 1 || TTCols == 1),
260  "Demux contains less than 4 outputs and/or does not map to a 1D object.");
261  return this->template o<3>();
262  }
263 
264  protected:
265  void bindEquation()
266  {
267  this->equation = ::std::bind(
268  &DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>::triggerFunction,
269  this,
270  ::std::placeholders::_1
271  );
272  }
273 
274  void triggerFunction(Base &b)
275  {
276  get();
277  }
278 
279  template<unsigned int TIdx = TRows * TCols - 1>
280  typename ::std::enable_if<(TIdx > 0), void>::type
281  get()
282  {
283  switch (getOperator()) {
284  case DemuxBlockOperator::RowMajor:
285  this->template o<TIdx>() = this->template i<0>().object(
286  floor<int, double>(TIdx / TCols),
287  TIdx -
288  floor<int, double>(TIdx / TCols) * TCols
289  );
290  break;
291  case DemuxBlockOperator::ColMajor:
292  this->template o<TIdx>() = this->template i<0>().object(
293  TIdx -
294  floor<int, double>(TIdx / TRows) * TRows,
295  floor<int, double>(TIdx / TRows));
296  break;
297  }
298  return get<TIdx - 1>();
299  }
300 
301  template<unsigned int TIdx = TRows * TCols - 1>
302  typename ::std::enable_if<(TIdx == 0), void>::type
303  get()
304  {
305  this->template o<TIdx>() = this->template i<0>().object(0,
306  0);
307  }
308 
309  template<unsigned int TIdx = TRows * TCols - 1>
310  typename ::std::enable_if<(TIdx < 0), void>::type
311  get()
312  {
313  return;
314  }
315  };
316 
317  template<typename... TTypes, DemuxBlockOperator TOps>
318  class DemuxBlock<::std::tuple<TTypes...>, TOps> :
319  public Block<
320  ::std::tuple<::std::tuple<TTypes...>>,
321  ::std::tuple<TTypes...>,
322  ::std::tuple<DemuxBlockOperator>
323  > {
324  public:
325  using Base =
326  Block<
327  ::std::tuple<::std::tuple<TTypes...>>,
328  ::std::tuple<TTypes...>,
329  ::std::tuple<DemuxBlockOperator>
330  >;
331 
332  DemuxBlock()
333  {
334  setOperator(TOps);
335  bindEquation();
336  }
337 
338  explicit DemuxBlock(const DemuxBlockOperator ops)
339  {
340  setOperator(ops);
341  bindEquation();
342  }
343 
344  void setOperator(const DemuxBlockOperator ops)
345  {
346  this->template p<0>() = ops;
347  }
348 
349  DemuxBlockOperator getOperator() const
350  {
351  return this->template p<0>();
352  }
353 
354  protected:
355  void bindEquation()
356  {
357  this->equation = ::std::bind(
358  &DemuxBlock<::std::tuple<TTypes...>, TOps>::triggerFunction,
359  this,
360  ::std::placeholders::_1
361  );
362  }
363 
364  void triggerFunction(Base &b)
365  {
366  get();
367  }
368 
369  template<unsigned int TIdx = Base::kOuts - 1>
370  typename ::std::enable_if<(TIdx > 0), void>::type
371  get()
372  {
373  this->template o<TIdx>() = ::std::get<TIdx>(
374  this->template i<0>().object);
375  return get<TIdx - 1>();
376  }
377 
378  template<unsigned int TIdx = Base::kOuts - 1>
379  typename ::std::enable_if<(TIdx == 0), void>::type
380  get()
381  {
382  this->template o<TIdx>() = ::std::get<TIdx>(
383  this->template i<0>().object);
384  }
385 
386  template<unsigned int TIdx = Base::kOuts - 1>
387  typename ::std::enable_if<(TIdx < 0), void>::type
388  get()
389  {
390  return;
391  }
392  };
393  }
394 
395 
396  template<typename TScalar, int TRows, int TCols, std::DemuxBlockOperator TOps>
397  class BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>> {
398  public:
399  static constexpr const BlockType blockType = BlockType::DemuxBlock;
400  enum {
401  directFeedthrough = true
402  };
403 
404  using type = std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>;
405  using Base = typename type::Base;
406 
407  enum {
408  kIns = Base::kIns,
409  kOuts = Base::kOuts,
410  kPars = Base::kPars
411  };
412 
413  static const ::std::array<::std::string, kIns> inTypes;
414  static const ::std::array<::std::string, kOuts> outTypes;
415  static const ::std::array<::std::string, kPars> parTypes;
416 
417  static const ::std::array<::std::string, 2> templateTypes;
418  };
419 
420  template<typename TScalar, int TRows, int TCols, std::DemuxBlockOperator TOps>
421  const ::std::array<::std::string, BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kIns> BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::inTypes =
422  {demangle(typeid(Eigen::Matrix<TScalar, TRows, TCols>).name())};
423 
424  template<typename TScalar, int TRows, int TCols, std::DemuxBlockOperator TOps>
425  const ::std::array<::std::string, BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kOuts> BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::outTypes =
426  ls::aux::TemplateTools::create_array<BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kOuts>(
427  demangle(typeid(TScalar).name())
428  );
429 
430  template<typename TScalar, int TRows, int TCols, std::DemuxBlockOperator TOps>
431  const ::std::array<::std::string, BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kPars> BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::parTypes =
432  {demangle(typeid(TOps).name())};
433 
434  template<typename TScalar, int TRows, int TCols, std::DemuxBlockOperator TOps>
435  const ::std::array<::std::string, 2> BlockTraits<std::DemuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::templateTypes =
436  {demangle(typeid(Eigen::Matrix<TScalar, TRows, TCols>).name()), demangle(typeid(TOps).name())};
437  }
438 }
439 
440 
441 #endif //LODESTAR_DEMUXBLOCK_HPP
ls::blocks::BlockTraits::directFeedthrough
static constexpr const bool directFeedthrough
Whether or not the block has direct feedthrough.
Definition: BlockTraits.hpp:44
ls::blocks::BlockTraits
A traits object that exposes information about TBlock.
Definition: BlockTraits.hpp:37
ls::blocks::BlockTraits::inTypes
static const ::std::array<::std::string, kIns > inTypes
Input types (as strings).
Definition: BlockTraits.hpp:59
ls::blocks::BlockType
BlockType
Block type information.
Definition: BlockType.hpp:25
ls::blocks::BlockType::DemuxBlock
@ DemuxBlock
Demultiplexer block.
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::aux::TemplateTools::repeat
Definition: TemplateTools.hpp:47
ls::blocks::Signal
Definition: Signal.hpp:22
ls::blocks::std::DemuxBlock< Eigen::Matrix< TScalar, TRows, TCols >, TOps >
Definition: DemuxBlock.hpp:34
ls::blocks::BlockTraits::outTypes
static const ::std::array<::std::string, kOuts > outTypes
Output types (as strings).
Definition: BlockTraits.hpp:61
ls::blocks::BlockTraits::blockType
static constexpr const BlockType blockType
Block type.
Definition: BlockTraits.hpp:42
ls::blocks::std::DemuxBlock
Definition: DemuxBlock.hpp:22
ls::blocks::BlockTraits::templateTypes
static const ::std::array<::std::string, 1 > templateTypes
Template parameter types (as strings).
Definition: BlockTraits.hpp:66
ls::blocks::Block
Generic base template class for all tuple-based Block instances.
Definition: Block.hpp:45
ls::blocks::BlockTraits::kIns
static const constexpr int kIns
Number of input slots.
Definition: BlockTraits.hpp:52
ls::blocks::BlockTraits::parTypes
static const ::std::array<::std::string, kPars > parTypes
Parameter types (as strings).
Definition: BlockTraits.hpp:63
ls::blocks::BlockTraits::kPars
static const constexpr int kPars
Number of parameters.
Definition: BlockTraits.hpp:56
ls::blocks::BlockTraits::kOuts
static const constexpr int kOuts
Number of output slots.
Definition: BlockTraits.hpp:54