PX4 Firmware
PX4 Autopilot Software http://px4.io
helper.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  // general wraps
9  TEST(fabs(wrap(4., 0., 10.) - 4.) < FLT_EPSILON);
10  TEST(fabs(wrap(4., 0., 1.)) < FLT_EPSILON);
11  TEST(fabs(wrap(-4., 0., 10.) - 6.) < FLT_EPSILON);
12  TEST(fabs(wrap(-18., 0., 10.) - 2.) < FLT_EPSILON);
13  TEST(fabs(wrap(-1.5, 3., 5.) - 4.5) < FLT_EPSILON);
14  TEST(fabs(wrap(15.5, 3., 5.) - 3.5) < FLT_EPSILON);
15  TEST(fabs(wrap(-1., 30., 40.) - 39.) < FLT_EPSILON);
16  TEST(fabs(wrap(-8000., -555., 1.) - (-216.)) < FLT_EPSILON);
17  TEST(fabs(wrap(0., 0., 360.)) < FLT_EPSILON);
18  TEST(fabs(wrap(0. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
19  TEST(fabs(wrap(0. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
20  TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
21  TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
22  TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
23 
24  // wrap pi
25  TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);
26  TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON);
27  TEST(fabs(wrap_pi(-4.) - (-4. + M_TWOPI)) < FLT_EPSILON);
28  TEST(fabs(wrap_pi(3.) - (3.)) < FLT_EPSILON);
29  TEST(fabs(wrap_pi(100.) - (100. - 32. * M_PI)) < FLT_EPSILON);
30  TEST(fabs(wrap_pi(-100.) - (-100. + 32. * M_PI)) < FLT_EPSILON);
31  TEST(fabs(wrap_pi(-101.) - (-101. + 32. * M_PI)) < FLT_EPSILON);
32  TEST(!is_finite(wrap_pi(NAN)));
33 
34  // wrap 2pi
35  TEST(fabs(wrap_2pi(0.)) < FLT_EPSILON);
36  TEST(fabs(wrap_2pi(-4.) - (-4. + 2. * M_PI)) < FLT_EPSILON);
37  TEST(fabs(wrap_2pi(3.) - (3.)) < FLT_EPSILON);
38  TEST(fabs(wrap_2pi(200.) - (200. - 31. * M_TWOPI)) < FLT_EPSILON);
39  TEST(fabs(wrap_2pi(-201.) - (-201. + 32. * M_TWOPI)) < FLT_EPSILON);
40  TEST(!is_finite(wrap_2pi(NAN)));
41 
42  // Equality checks
43  TEST(isEqualF(1., 1.));
44  TEST(!isEqualF(1., 2.));
45  TEST(!isEqualF(NAN, 1.f));
46  TEST(!isEqualF(1.f, NAN));
47  TEST(!isEqualF(INFINITY, 1.f));
48  TEST(!isEqualF(1.f, INFINITY));
49  TEST(isEqualF(NAN, NAN));
50  TEST(isEqualF(NAN, -NAN));
51  TEST(isEqualF(-NAN, NAN));
52  TEST(isEqualF(INFINITY, INFINITY));
53  TEST(!isEqualF(INFINITY, -INFINITY));
54  TEST(!isEqualF(-INFINITY, INFINITY));
55  TEST(isEqualF(-INFINITY, -INFINITY));
56 
57  Vector3f a(1, 2, 3);
58  Vector3f b(4, 5, 6);
59  TEST(!isEqual(a, b));
60  TEST(isEqual(a, a));
61 
62  Vector3f c(1, 2, 3);
63  Vector3f d(1, 2, NAN);
64  TEST(!isEqual(c, d));
65  TEST(isEqual(c, c));
66  TEST(isEqual(d, d));
67 
68  return 0;
69 }
70 
71 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
matrix::wrap
Type wrap(Type x, Type low, Type high)
Wrap value to stay in range [low, high)
Definition: helper_functions.hpp:51
M_TWOPI
#define M_TWOPI
Definition: stdlib_imports.hpp:20
FLT_EPSILON
#define FLT_EPSILON
Definition: stdlib_imports.hpp:26
matrix::wrap_2pi
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
Definition: helper_functions.hpp:76
main
int main()
Definition: helper.cpp:6
matrix::isEqual
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
matrix::is_finite
bool is_finite(Type x)
Definition: helper_functions.hpp:13
matrix::wrap_pi
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Definition: helper_functions.hpp:67
test_macros.hpp
TEST
#define TEST(X)
Definition: test_macros.hpp:14
M_PI
#define M_PI
Definition: gps_helper.cpp:38
matrix::isEqualF
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Definition: helper_functions.hpp:35