PX4 Firmware
PX4 Autopilot Software http://px4.io
matrixMult.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 int main()
7 {
8  float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
9  Matrix3f A(data);
10 
11  float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
12  Matrix3f A_I(data_check);
13  Matrix3f I;
14  I.setIdentity();
15  Matrix3f R = A * A_I;
16  TEST(isEqual(R, I));
17 
18  Matrix3f R2 = A;
19  R2 *= A_I;
20  TEST(isEqual(R2, I));
21 
22  TEST(R2==I);
23  TEST(A!=A_I);
24  Matrix3f A2 = eye<float, 3>()*2;
25  Matrix3f B = A2.emult(A2);
26  Matrix3f B_check = eye<float, 3>()*4;
27  Matrix3f C_check = eye<float, 3>()*2;
28  TEST(isEqual(B, B_check));
29  Matrix3f C = B_check.edivide(C_check);
30 
31  float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2};
32  // off diagonal are NANs because division by 0
33  TEST(C == Matrix3f(off_diagonal_nan));
34 
35  // Test non-square matrix
36  float data_43[12] = {1,3,2,
37  2,2,1,
38  5,2,1,
39  2,3,4
40  };
41  float data_32[6] = {2,3,
42  1,7,
43  5,4
44  };
45 
46  Matrix<float, 4, 3> m43(data_43);
47  Matrix<float, 3, 2> m32(data_32);
48 
49  Matrix<float, 4, 2> m42 = m43 * m32;
50 
51  float data_42[8] = {15,32,
52  11,24,
53  17,33,
54  27,43
55  };
56  Matrix<float, 4, 2> m42_check(data_42);
57  TEST(isEqual(m42, m42_check))
58 
59  return 0;
60 }
61 
62 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
SquareMatrix< float, 3 > Matrix3f
void setIdentity()
Definition: Matrix.hpp:447
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
Matrix< Type, M, N > edivide(const Matrix< Type, M, N > &other) const
Definition: Matrix.hpp:171
#define TEST(X)
Definition: test_macros.hpp:14
int main()
Definition: matrixMult.cpp:6
Matrix< Type, M, N > emult(const Matrix< Type, M, N > &other) const
Definition: Matrix.hpp:157