Lodestar
An integrated real-time control package in C++
MuxBlock.hpp
1 //
2 // Created by Hamza El-Kebir on 12/23/21.
3 //
4 
5 #ifndef LODESTAR_MUXBLOCK_HPP
6 #define LODESTAR_MUXBLOCK_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 MuxBlockOperator {
17  RowMajor,
18  ColMajor
19  };
20 
21  template<typename TType, MuxBlockOperator TOps = MuxBlockOperator::RowMajor>
22  class MuxBlock :
23  public Block<
24  ::std::tuple<TType>,
25  ::std::tuple<TType>,
26  BlockProto::empty
27  > {
28  static_assert(true,
29  "MuxBlock not defined for this type.");
30  };
31 
32  // TODO: Look into support for dynamically size matrices.
33  template<typename TScalar, int TRows, int TCols, MuxBlockOperator TOps>
34  class MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps> :
35  public Block<
36  typename ls::aux::TemplateTools::repeat<TScalar,
37  TRows * TCols, ::std::tuple>::type,
38  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
39  ::std::tuple<MuxBlockOperator>
40  > {
41  public:
42  using Base =
43  Block<
44  typename ls::aux::TemplateTools::repeat<TScalar,
45  TRows * TCols, ::std::tuple>::type,
46  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
47  ::std::tuple<MuxBlockOperator>
48  >;
49 
50  MuxBlock()
51  {
52  setOperator(TOps);
53  bindEquation();
54  }
55 
56  explicit MuxBlock(const MuxBlockOperator ops)
57  {
58  setOperator(ops);
59  bindEquation();
60  }
61 
62  void setOperator(const MuxBlockOperator ops)
63  {
64  this->template p<0>() = ops;
65  }
66 
67  MuxBlockOperator 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  "Mux contains less than 1 input 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  "Mux contains less than 1 input 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  "Mux contains less than 2 inputs 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  "Mux contains less than 2 inputs 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 i<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 * TCols > 2) &&
187  (TTRows == 1 || TTCols == 1),
188  "Mux contains less than 3 inputs and/or does not map to a 1D object.");
189  return this->template i<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 i<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  "Mux contains less than 3 inputs and/or does not map to a 1D object.");
213  return this->template i<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 i<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  "Mux contains less than 4 inputs and/or does not map to a 1D object.");
237  return this->template i<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 i<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  "Mux contains less than 4 inputs and/or does not map to a 1D object.");
261  return this->template i<3>();
262  }
263 
264  protected:
265  void bindEquation()
266  {
267  this->equation = ::std::bind(
268  &MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>::triggerFunction,
269  this,
270  ::std::placeholders::_1
271  );
272  }
273 
274  void triggerFunction(Base &b)
275  {
276  set();
277  }
278 
279  template<unsigned int TIdx = TRows * TCols - 1>
280  typename ::std::enable_if<(TIdx > 0), void>::type
281  set()
282  {
283  switch (getOperator()) {
284  case MuxBlockOperator::RowMajor:
285  this->template o<0>().object(
286  floor<int, double>(TIdx / TCols),
287  TIdx -
288  floor<int, double>(TIdx / TCols) * TCols)
289  = this->template i<TIdx>();
290  break;
291  case MuxBlockOperator::ColMajor:
292  this->template o<0>().object(
293  TIdx -
294  floor<int, double>(TIdx / TRows) * TRows,
295  floor<int, double>(TIdx / TRows))
296  = this->template i<TIdx>();
297  break;
298  }
299  return set<TIdx - 1>();
300  }
301 
302  template<unsigned int TIdx = TRows * TCols - 1>
303  typename ::std::enable_if<(TIdx == 0), void>::type
304  set()
305  {
306  this->template o<0>().object(0,
307  0) = this->template i<TIdx>();
308  }
309 
310  template<unsigned int TIdx = TRows * TCols - 1>
311  typename ::std::enable_if<(TIdx < 0), void>::type
312  set()
313  {
314  return;
315  }
316  };
317 
318  template<typename... TTypes, MuxBlockOperator TOps>
319  class MuxBlock<::std::tuple<TTypes...>, TOps> :
320  public Block<
321  ::std::tuple<TTypes...>,
322  ::std::tuple<::std::tuple<TTypes...>>,
323  ::std::tuple<MuxBlockOperator>
324  > {
325  public:
326  using Base =
327  Block<
328  ::std::tuple<TTypes...>,
329  ::std::tuple<::std::tuple<TTypes...>>,
330  ::std::tuple<MuxBlockOperator>
331  >;
332 
333  MuxBlock()
334  {
335  setOperator(TOps);
336  bindEquation();
337  }
338 
339  explicit MuxBlock(const MuxBlockOperator ops)
340  {
341  setOperator(ops);
342  bindEquation();
343  }
344 
345  void setOperator(const MuxBlockOperator ops)
346  {
347  this->template p<0>() = ops;
348  }
349 
350  MuxBlockOperator getOperator() const
351  {
352  return this->template p<0>();
353  }
354 
355  protected:
356  void bindEquation()
357  {
358  this->equation = ::std::bind(
359  &MuxBlock<::std::tuple<TTypes...>, TOps>::triggerFunction,
360  this,
361  ::std::placeholders::_1
362  );
363  }
364 
365  void triggerFunction(Base &b)
366  {
367  set();
368  }
369 
370  template<unsigned int TIdx = Base::kIns - 1>
371  typename ::std::enable_if<(TIdx > 0), void>::type
372  set()
373  {
374  ::std::get<TIdx>(
375  this->template o<0>().object) = this->template i<TIdx>().object;
376  return set<TIdx - 1>();
377  }
378 
379  template<unsigned int TIdx = Base::kIns - 1>
380  typename ::std::enable_if<(TIdx == 0), void>::type
381  set()
382  {
383  ::std::get<TIdx>(
384  this->template o<0>().object) = this->template i<TIdx>().object;
385  this->template o<0>().propagate();
386  }
387 
388  template<unsigned int TIdx = Base::kIns - 1>
389  typename ::std::enable_if<(TIdx < 0), void>::type
390  set()
391  {
392  return;
393  }
394  };
395  }
396 
397  template<typename TScalar, int TRows, int TCols, std::MuxBlockOperator TOps>
398  class BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>> {
399  public:
400  static constexpr const BlockType blockType = BlockType::MuxBlock;
401  enum {
402  directFeedthrough = true
403  };
404 
405  using type = std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>;
406  using Base = typename type::Base;
407 
408  enum {
409  kIns = Base::kIns,
410  kOuts = Base::kOuts,
411  kPars = Base::kPars
412  };
413 
414  static const ::std::array<::std::string, kIns> inTypes;
415  static const ::std::array<::std::string, kOuts> outTypes;
416  static const ::std::array<::std::string, kPars> parTypes;
417 
418  static const ::std::array<::std::string, 2> templateTypes;
419  };
420 
421  template<typename TScalar, int TRows, int TCols, std::MuxBlockOperator TOps>
422  const ::std::array<::std::string, BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kIns> BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::inTypes =
423  ls::aux::TemplateTools::create_array<TRows * TCols>(
424  demangle(typeid(TScalar).name())
425  );
426 
427  template<typename TScalar, int TRows, int TCols, std::MuxBlockOperator TOps>
428  const ::std::array<::std::string, BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kOuts> BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::outTypes =
429  { demangle(typeid(Eigen::Matrix<TScalar, TRows, TCols>).name()) };
430 
431  template<typename TScalar, int TRows, int TCols, std::MuxBlockOperator TOps>
432  const ::std::array<::std::string, BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::kPars> BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::parTypes =
433  { demangle(typeid(TOps).name()) };
434 
435  template<typename TScalar, int TRows, int TCols, std::MuxBlockOperator TOps>
436  const ::std::array<::std::string, 2> BlockTraits<std::MuxBlock<Eigen::Matrix<TScalar, TRows, TCols>, TOps>>::templateTypes =
437  { demangle(typeid(Eigen::Matrix<TScalar, TRows, TCols>).name()), demangle(typeid(TOps).name()) };
438  }
439 }
440 
441 
442 #endif //LODESTAR_MUXBLOCK_HPP
ls::blocks::BlockTraits::directFeedthrough
static constexpr const bool directFeedthrough
Whether or not the block has direct feedthrough.
Definition: BlockTraits.hpp:44
ls::blocks::std::MuxBlock
Definition: MuxBlock.hpp:22
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::MuxBlock
@ MuxBlock
Multiplexer 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::BlockTraits::outTypes
static const ::std::array<::std::string, kOuts > outTypes
Output types (as strings).
Definition: BlockTraits.hpp:61
ls::blocks::std::MuxBlock< Eigen::Matrix< TScalar, TRows, TCols >, TOps >
Definition: MuxBlock.hpp:34
ls::blocks::BlockTraits::blockType
static constexpr const BlockType blockType
Block type.
Definition: BlockTraits.hpp:42
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