PX4 Firmware
PX4 Autopilot Software http://px4.io
copyto.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 namespace {
7 void doTheCopy(const Matrix<float, 2, 3>& A, float array_A[6])
8 {
9  A.copyTo(array_A);
10 }
11 }
12 
13 int main()
14 {
15  float eps = 1e-6f;
16 
17  // Vector3 copyTo
18  Vector3f v(1, 2, 3);
19  float dst3[3] = {};
20  v.copyTo(dst3);
21  for (size_t i = 0; i < 3; i++) {
22  TEST(fabs(v(i) - dst3[i]) < eps);
23  }
24 
25  // Quaternion copyTo
26  Quatf q(1, 2, 3, 4);
27  float dst4[4] = {};
28  q.copyTo(dst4);
29  for (size_t i = 0; i < 4; i++) {
30  TEST(fabs(q(i) - dst4[i]) < eps);
31  }
32 
33  // Matrix copyTo
35  A(0,0) = 1;
36  A(0,1) = 2;
37  A(0,2) = 3;
38  A(1,0) = 4;
39  A(1,1) = 5;
40  A(1,2) = 6;
41  float array_A[6] = {};
42  doTheCopy(A, array_A);
43  float array_row[6] = {1, 2, 3, 4, 5, 6};
44  for (size_t i = 0; i < 6; i++) {
45  TEST(fabs(array_A[i] - array_row[i]) < eps);
46  }
47 
48  // Matrix copyToColumnMajor
49  A.copyToColumnMajor(array_A);
50  float array_column[6] = {1, 4, 2, 5, 3, 6};
51  for (size_t i = 0; i < 6; i++) {
52  TEST(fabs(array_A[i] - array_column[i]) < eps);
53  }
54 
55  // Slice copyTo
56  float dst5[2] = {};
57  v.slice<2,1>(0,0).copyTo(dst5);
58  for (size_t i = 0; i < 2; i++) {
59  TEST(fabs(v(i) - dst5[i]) < eps);
60  }
61 
62  float subarray_A[4] = {};
63  A.slice<2,2>(0,0).copyToColumnMajor(subarray_A);
64  float subarray_column[4] = {1,4,2,5};
65  for (size_t i = 0; i < 4; i++) {
66  TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
67  }
68 
69  return 0;
70 }
71 
72 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
void copyTo(Type dst[M *N]) const
Definition: Matrix.hpp:115
Quaternion class.
Definition: Dcm.hpp:24
const Slice< Type, P, Q, M, N > slice(size_t x0, size_t y0) const
Definition: Matrix.hpp:374
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
int main()
Definition: copyto.cpp:13
#define TEST(X)
Definition: test_macros.hpp:14
void copyToColumnMajor(Type dst[M *N]) const
Definition: Matrix.hpp:120