PX4 Firmware
PX4 Autopilot Software http://px4.io
slice.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 #include <matrix/math.hpp>
3 
4 using namespace matrix;
5 
6 template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage results
7 
8 
9 int main()
10 {
11  float data[9] = {0, 2, 3,
12  4, 5, 6,
13  7, 8, 10
14  };
15  SquareMatrix<float, 3> A(data);
16 
17  // Test row slicing
18  Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
19  float data_check_rowslice[6] = {
20  4, 5, 6,
21  7, 8, 10
22  };
23  Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
24  TEST(isEqual(B_rowslice, B_check_rowslice));
25 
26  // Test column slicing
27  Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
28  float data_check_colslice[6] = {
29  2, 3,
30  5, 6,
31  8, 10
32  };
33  Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
34  TEST(isEqual(B_colslice, B_check_colslice));
35 
36  // Test slicing both
37  Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
38  float data_check_bothslice[4] = {
39  5, 6,
40  8, 10
41  };
42  Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
43  TEST(isEqual(B_bothslice, B_check_bothslice));
44 
45  //Test block writing
46  float data_2[4] = {
47  11, 12,
48  13, 14
49  };
50 
51  Matrix<float, 2, 2> C(data_2);
52  A.slice<2, 2>(1, 1) = C;
53 
54  float data_2_check[9] = {
55  0, 2, 3,
56  4, 11, 12,
57  7, 13, 14
58  };
59  Matrix<float, 3, 3> D(data_2_check);
60  TEST(isEqual(A, D));
61 
62  //Test writing to slices
64  E(0,0) = -1;
65  E(1,0) = 1;
66  E(2,0) = 3;
67 
69  F(0,0) = 9;
70  F(1,0) = 11;
71 
72  E.slice<2,1>(0,0) = F;
73 
74  float data_3_check[3] = {9, 11, 3};
75  Matrix<float, 3, 1> G (data_3_check);
76  TEST(isEqual(E, G));
77  TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
78 
79  Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
80  TEST(isEqual(H,F));
81 
82  float data_4_check[5] = {3, 11, 9, 0, 0};
83  { // assigning row slices to each other
84  const Matrix<float, 3, 1> J (data_3_check);
86  K.row(2) = J.row(0);
87  K.row(1) = J.row(1);
88  K.row(0) = J.row(2);
89 
90  Matrix<float, 5, 1> K_check(data_4_check);
91  TEST(isEqual(K, K_check));
92  }
93  { // assigning col slices to each other
94  const Matrix<float, 1, 3> J (data_3_check);
96  K.col(2) = J.col(0);
97  K.col(1) = J.col(1);
98  K.col(0) = J.col(2);
99 
100  Matrix<float, 1, 5> K_check(data_4_check);
101  TEST(isEqual(K, K_check));
102  }
103 
104  // check that slice of a slice works for reading
105  const Matrix<float, 3, 3> cm33(data);
106  Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2);
107  float top_right_check[2] = {3,6};
108  TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
109 
110  // check that slice of a slice works for writing
111  Matrix<float, 3, 3> m33(data);
112  m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>();
113  const float data_check[9] = {0, 2, 0,
114  4, 5, 0,
115  7, 8, 10
116  };
117  TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
118 
119  // longerThan
120  Vector3f v5;
121  v5(0) = 3;
122  v5(1) = 4;
123  v5(2) = 9;
124  TEST(v5.xy().longerThan(4.99f));
125  TEST(!v5.xy().longerThan(5.f));
126  TEST(isEqualF(5.f, v5.xy().norm()));
127 
128  return 0;
129 }
130 
131 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
int main()
Definition: slice.cpp:9
const Slice< Type, M, 1, M, N > col(size_t j) const
Definition: Matrix.hpp:395
const Slice< Type, P, Q, M, N > slice(size_t x0, size_t y0) const
Definition: Matrix.hpp:374
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
uint8_t * data
Definition: dataman.cpp:149
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define TEST(X)
Definition: test_macros.hpp:14
const Slice< Type, 1, N, M, N > row(size_t i) const
Definition: Matrix.hpp:385
const Slice< Type, 2, 1, 3, 1 > xy() const
Definition: Vector3.hpp:108
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.