PX4 Firmware
PX4 Autopilot Software http://px4.io
Slice.hpp
Go to the documentation of this file.
1 /**
2  * @file Slice.hpp
3  *
4  * A simple matrix template library.
5  *
6  * @author Julian Kent < julian@auterion.com >
7  */
8 
9 #pragma once
10 
11 #include "math.hpp"
12 
13 
14 namespace matrix {
15 
16 template<typename Type, size_t M, size_t N>
17 class Matrix;
18 
19 template<typename Type, size_t M>
20 class Vector;
21 
22 template <typename Type, size_t P, size_t Q, size_t M, size_t N>
23 class Slice {
24 public:
25  Slice(size_t x0, size_t y0, const Matrix<Type, M, N>* data) :
26  _x0(x0),
27  _y0(y0),
28  _data(const_cast<Matrix<Type, M, N>*>(data)) {
29  static_assert(P <= M, "Slice rows bigger than backing matrix");
30  static_assert(Q <= N, "Slice cols bigger than backing matrix");
31  }
32 
33  Type operator()(size_t i, size_t j) const
34  {
35  return (*_data)(_x0 + i, _y0 + j);
36  }
37 
38  Type &operator()(size_t i, size_t j)
39  {
40  return (*_data)(_x0 + i, _y0 + j);
41  }
42 
43  template<size_t MM, size_t NN>
45  {
46  Slice<Type, P, Q, M, N>& self = *this;
47  for (size_t i = 0; i < P; i++) {
48  for (size_t j = 0; j < Q; j++) {
49  self(i, j) = other(i, j);
50  }
51  }
52  return self;
53  }
54 
56  {
57  Slice<Type, P, Q, M, N>& self = *this;
58  for (size_t i = 0; i < P; i++) {
59  for (size_t j = 0; j < Q; j++) {
60  self(i, j) = other(i, j);
61  }
62  }
63  return self;
64  }
65 
66  // allow assigning vectors to a slice that are in the axis
67  template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
69  {
70  Slice<Type, 1, Q, M, N>& self = *this;
71  for (size_t j = 0; j < Q; j++) {
72  self(0, j) = other(j);
73  }
74  return self;
75  }
76 
77  template<size_t R, size_t S>
78  const Slice<Type, R, S, M, N> slice(size_t x0, size_t y0) const
79  {
80  return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
81  }
82 
83  template<size_t R, size_t S>
84  Slice<Type, R, S, M, N> slice(size_t x0, size_t y0)
85  {
86  return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
87  }
88 
89  void copyTo(Type dst[M*N]) const
90  {
91  const Slice<Type, P, Q, M, N> &self = *this;
92 
93  for (size_t i = 0; i < M; i++) {
94  for (size_t j = 0; j < N; j++) {
95  dst[i*N+j] = self(i, j);
96  }
97  }
98  }
99 
100  void copyToColumnMajor(Type dst[M*N]) const
101  {
102  const Slice<Type, P, Q, M, N> &self = *this;
103 
104  for (size_t i = 0; i < M; i++) {
105  for (size_t j = 0; j < N; j++) {
106  dst[i+(j*M)] = self(i, j);
107  }
108  }
109  }
110 
112  {
113  Slice<Type, P, Q, M, N>& self = *this;
114  Type accum(0);
115  for (size_t i = 0; i < P; i++) {
116  for (size_t j = 0; j < Q; j++) {
117  accum += self(i, j)*self(i, j);
118  }
119  }
120  return accum;
121  }
122 
123  Type norm()
124  {
125  return matrix::sqrt(norm_squared());
126  }
127 
128  bool longerThan(Type testVal)
129  {
130  return norm_squared() > testVal*testVal;
131  }
132 
133 private:
134  size_t _x0, _y0;
136 };
137 
138 }
Matrix< Type, M, N > * _data
Definition: Slice.hpp:135
size_t _y0
Definition: Slice.hpp:134
Type operator()(size_t i, size_t j) const
Definition: Slice.hpp:33
const Slice< Type, R, S, M, N > slice(size_t x0, size_t y0) const
Definition: Slice.hpp:78
bool longerThan(Type testVal)
Definition: Slice.hpp:128
size_t _x0
Definition: Slice.hpp:134
void copyTo(Type dst[M *N]) const
Definition: Slice.hpp:89
uint8_t * data
Definition: dataman.cpp:149
Type norm_squared()
Definition: Slice.hpp:111
Slice< Type, P, Q, M, N > & operator=(const Matrix< Type, P, Q > &other)
Definition: Slice.hpp:55
Slice(size_t x0, size_t y0, const Matrix< Type, M, N > *data)
Definition: Slice.hpp:25
Type & operator()(size_t i, size_t j)
Definition: Slice.hpp:38
Type norm()
Definition: Slice.hpp:123
Slice< Type, R, S, M, N > slice(size_t x0, size_t y0)
Definition: Slice.hpp:84
Slice< Type, P, Q, M, N > & operator=(const Slice< Type, P, Q, MM, NN > &other)
Definition: Slice.hpp:44
P[0][0]
Definition: quatCovMat.c:44
void copyToColumnMajor(Type dst[M *N]) const
Definition: Slice.hpp:100
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Definition: Dual.hpp:188
Slice< Type, 1, Q, M, N > & operator=(const Vector< Type, Q > &other)
Definition: Slice.hpp:68