PX4 Firmware
PX4 Autopilot Software http://px4.io
matrixAssignment.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::Matrix<float, 3, 2>;
7 
8 int main()
9 {
10  Matrix3f m;
11  m.setZero();
12  m.zero();
13  m(0, 0) = 1;
14  m(0, 1) = 2;
15  m(0, 2) = 3;
16  m(1, 0) = 4;
17  m(1, 1) = 5;
18  m(1, 2) = 6;
19  m(2, 0) = 7;
20  m(2, 1) = 8;
21  m(2, 2) = 9;
22 
23  float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
24  Matrix3f m2(data);
25 
26  for(size_t i=0; i<3; i++) {
27  for (size_t j = 0; j < 3; j++) {
28  TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
29  }
30  }
31 
32  Matrix3f m_nan;
33  m_nan.setNaN();
34  for(size_t i=0; i<3; i++) {
35  for (size_t j = 0; j < 3; j++) {
36  TEST(isnan(m_nan(i,j)));
37  }
38  }
39  TEST(m_nan.isAllNan());
40 
41  float data2d[3][3] = {
42  {1, 2, 3},
43  {4, 5, 6},
44  {7, 8, 9}
45  };
46  m2 = Matrix3f(data2d);
47  for(size_t i=0; i<3; i++) {
48  for (size_t j = 0; j < 3; j++) {
49  TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
50  }
51  }
52  TEST(!m2.isAllNan());
53 
54  float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
55  Matrix3f m3(data_times_2);
56 
57  TEST(isEqual(m, m2));
58  TEST(!(isEqual(m, m3)));
59 
60  m2 *= 2;
61  TEST(isEqual(m2, m3));
62 
63  m2 /= 2;
64  m2 -= 1;
65  float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
66  TEST(isEqual(Matrix3f(data_minus_1), m2));
67 
68  m2 += 1;
69  TEST(isEqual(Matrix3f(data), m2));
70 
71  m3 -= m2;
72 
73  TEST(isEqual(m3, m2));
74 
75  float data_row_02_swap[9] = {
76  7, 8, 9,
77  4, 5, 6,
78  1, 2, 3,
79  };
80 
81  float data_col_02_swap[9] = {
82  3, 2, 1,
83  6, 5, 4,
84  9, 8, 7
85  };
86 
87  Matrix3f m4(data);
88 
89  TEST(isEqual(-m4, m4*(-1)));
90 
91  // col swap
92  m4.swapCols(0, 2);
93  TEST(isEqual(m4, Matrix3f(data_col_02_swap)));
94  m4.swapCols(0, 2);
95 
96  // row swap
97  m4.swapRows(0, 2);
98  TEST(isEqual(m4, Matrix3f(data_row_02_swap)));
99  m4.swapRows(0, 2);
100 
101  // swapping with same row should do nothing
102  m4.swapRows(0, 0);
103  m4.swapRows(1, 1);
104  m4.swapRows(2, 2);
105  TEST(isEqual(m4, Matrix3f(data)));
106 
107  // swapping with same col should do nothing
108  m4.swapCols(0, 0);
109  m4.swapCols(1, 1);
110  m4.swapCols(2, 2);
111  TEST(isEqual(m4, Matrix3f(data)));
112 
113  TEST(fabs(m4.min() - 1) < FLT_EPSILON);
114  TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
115 
116  Scalar<float> s = 1;
117  const Vector<float, 1> & s_vect = s;
118  TEST(fabs(s - 1) < FLT_EPSILON);
119  TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
120 
121  Matrix<float, 1, 1> m5 = s;
122  TEST(fabs(m5(0,0) - s) < FLT_EPSILON);
123 
125  m6.setRow(0, Vector2f(1, 2));
126  float m7_array[] = {1,2,0,0};
127  Matrix<float, 2, 2> m7(m7_array);
128  TEST(isEqual(m6, m7));
129  m6.setCol(0, Vector2f(3, 4));
130  float m8_array[] = {3,2,4,0};
131  Matrix<float, 2, 2> m8(m8_array);
132  TEST(isEqual(m6, m8));
133 
134  m7.setNaN();
135  TEST(m7 != m8);
136 
137  // min, max, constrain matrix values with scalar
138  float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
139  float lower_bound = 7;
140  float upper_bound = 11;
141  float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18};
142  float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11};
143  float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11};
144  Matrix3f m9(data_m9);
145  Matrix3f m9_lower_bounded(data_m9_lower_bounded);
146  Matrix3f m9_upper_bounded(data_m9_upper_bounded);
147  Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained);
148  TEST(isEqual(max(m9, lower_bound), m9_lower_bounded));
149  TEST(isEqual(max(lower_bound, m9), m9_lower_bounded));
150  TEST(isEqual(min(m9, upper_bound), m9_upper_bounded));
151  TEST(isEqual(min(upper_bound, m9), m9_upper_bounded));
152  TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained));
153  TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan));
154 
155  // min, max, constrain matrix values with matrix of same size
156  float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
157  float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18};
158  float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18};
159  float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18};
160  float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18};
161  float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18};
162  Matrix3f m10(data_m10);
163  Matrix3f m10_lower_bound(data_m10_lower_bound);
164  Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref);
165  Matrix3f m10_upper_bound(data_m10_upper_bound);
166  Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref);
167  Matrix3f m10_constrained_ref(data_m10_constrained_ref);
168  TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref));
169  TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref));
170  TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref));
171  TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref));
172  TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref));
173 
174  // min, max, constrain with NAN
175  TEST(isEqualF(matrix::typeFunction::min(5.0f,NAN), 5.0f));
176  TEST(isEqualF(matrix::typeFunction::min(NAN,5.0f), 5.0f));
177  TEST(isEqualF(matrix::typeFunction::min(NAN,NAN), NAN));
178  TEST(isEqualF(matrix::typeFunction::max(5.0f,NAN), 5.0f));
179  TEST(isEqualF(matrix::typeFunction::max(NAN,5.0f), 5.0f));
180  TEST(isEqualF(matrix::typeFunction::max(NAN,NAN), NAN));
181  TEST(isEqualF(matrix::typeFunction::constrain(NAN,5.0f,6.0f), NAN));
185  Vector2f v1{NAN, 5.0f};
186  Vector2f v1_min = min(v1,1.0f);
187  Matrix3f m11 = min(m10_constrained_ref,NAN);
188  TEST(isEqualF(fmin(NAN,1.0f), float(v1_min(0))));
189  TEST(isEqual(m11, m10_constrained_ref));
190 
191  // check write_string()
192  float comma[6] = {
193  1.f, 12345.678f,
194  12345.67891f, 12345.67891f,
195  1112345.67891f, 12345.111111111f
196  };
197  Matrix<float, 3, 2> Comma(comma);
198  const size_t len = 10*2*3 + 2 + 1;
199  char buffer[len];
200  Comma.write_string(buffer, len);
201  char output[] = "\t 1\t12345.678\n\t12345.679\t12345.679\n\t1112345.6\t12345.111\n";
202  for (size_t i = 0; i < len; i++) {
203  TEST(buffer[i] == output[i]);
204  if (buffer[i] == '\0') {
205  break;
206  }
207  }
208 
209  // check print()
210  // write
211  FILE *fp = fopen("testoutput.txt", "w+");
212  TEST(fp != nullptr);
213  Comma.print(fp);
214  TEST(!fclose(fp));
215  // read
216  fp = fopen("testoutput.txt", "r");
217  TEST(fp != nullptr);
218  TEST(!fseek(fp, 0, SEEK_SET));
219  for (size_t i = 0; i < len; i++) {
220  char c = static_cast<char>(fgetc(fp));
221  if (c == '\n') {
222  break;
223  }
224  printf("%d %d %c\n", static_cast<int>(i), c, c);
225  TEST(c == output[i]);
226  }
227  TEST(!fclose(fp));
228 
229  return 0;
230 }
231 
232 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
void write_string(char *buf, size_t n) const
Misc.
Definition: Matrix.hpp:331
SquareMatrix< float, 3 > Matrix3f
bool isAllNan() const
Definition: Matrix.hpp:531
Matrix< Type, M, N > constrain(const Matrix< Type, M, N > &x, const Type scalar_lower_bound, const Type scalar_upper_bound)
Definition: Matrix.hpp:684
void setCol(size_t j, const Matrix< Type, M, 1 > &column)
Definition: Matrix.hpp:411
Type min(const Type x, const Type y)
Definition: Matrix.hpp:586
#define FLT_EPSILON
void swapRows(size_t a, size_t b)
Definition: Matrix.hpp:462
void setRow(size_t i, const Matrix< Type, N, 1 > &row_in)
Definition: Matrix.hpp:405
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
void print(FILE *stream=stdout) const
Definition: Matrix.hpp:343
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector2< float > Vector2f
Definition: Vector2.hpp:69
Dual< Scalar, N > min(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:231
void zero()
Definition: Matrix.hpp:421
void setNaN()
Definition: Matrix.hpp:442
#define TEST(X)
Definition: test_macros.hpp:14
int main()
Type min() const
Definition: Matrix.hpp:517
Dual< Scalar, N > max(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:224
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
void setZero()
Definition: Matrix.hpp:416
void swapCols(size_t a, size_t b)
Definition: Matrix.hpp:477
Type constrain(const Type x, const Type lower_bound, const Type upper_bound)
Definition: Matrix.hpp:618
Type max(const Type x, const Type y)
Definition: Matrix.hpp:602