APM:Libraries
test_math.cpp
Go to the documentation of this file.
1 #include <AP_gtest.h>
2 
3 #include <AP_Math/AP_Math.h>
4 
5 #define SQRT_2 1.4142135623730951f
6 
7 TEST(VectorTest, Rotations)
8 {
9  unsigned rotation_count = 0;
10 
11 #define TEST_ROTATION(rotation, _x, _y, _z) { \
12  const float accuracy = 1.0e-6; \
13  Vector3f v(1, 1, 1); \
14  v.rotate(rotation); \
15  Vector3f expected(_x, _y, _z); \
16  EXPECT_NEAR(expected.length(), v.length(), accuracy); \
17  EXPECT_FLOAT_EQ(expected.x, v.x); \
18  EXPECT_FLOAT_EQ(expected.y, v.y); \
19  EXPECT_FLOAT_EQ(expected.z, v.z); \
20  rotation_count++; \
21 }
22 
23  TEST_ROTATION(ROTATION_NONE, 1, 1, 1);
24  TEST_ROTATION(ROTATION_YAW_45, 0, SQRT_2, 1);
25  TEST_ROTATION(ROTATION_YAW_90, -1, 1, 1);
26  TEST_ROTATION(ROTATION_YAW_135, -SQRT_2, 0, 1);
27  TEST_ROTATION(ROTATION_YAW_180, -1, -1, 1);
28  TEST_ROTATION(ROTATION_YAW_225, 0, -SQRT_2, 1);
29  TEST_ROTATION(ROTATION_YAW_270, 1, -1, 1);
30  TEST_ROTATION(ROTATION_YAW_315, SQRT_2, 0, 1);
31  TEST_ROTATION(ROTATION_ROLL_180, 1, -1, -1);
32  TEST_ROTATION(ROTATION_ROLL_180_YAW_45, SQRT_2, 0, -1);
33  TEST_ROTATION(ROTATION_ROLL_180_YAW_90, 1, 1, -1);
34  TEST_ROTATION(ROTATION_ROLL_180_YAW_135, 0, SQRT_2, -1);
35  TEST_ROTATION(ROTATION_PITCH_180, -1, 1, -1);
36  TEST_ROTATION(ROTATION_ROLL_180_YAW_225, -SQRT_2, 0, -1);
37  TEST_ROTATION(ROTATION_ROLL_180_YAW_270, -1, -1, -1);
38  TEST_ROTATION(ROTATION_ROLL_180_YAW_315, 0, -SQRT_2, -1);
39  TEST_ROTATION(ROTATION_ROLL_90, 1, -1, 1);
40  TEST_ROTATION(ROTATION_ROLL_90_YAW_45, SQRT_2, 0, 1);
41  TEST_ROTATION(ROTATION_ROLL_90_YAW_90, 1, 1, 1);
42  TEST_ROTATION(ROTATION_ROLL_90_YAW_135, 0, SQRT_2, 1);
43  TEST_ROTATION(ROTATION_ROLL_270, 1, 1, -1);
44  TEST_ROTATION(ROTATION_ROLL_270_YAW_45, 0, SQRT_2, -1);
45  TEST_ROTATION(ROTATION_ROLL_270_YAW_90, -1, 1, -1);
46  TEST_ROTATION(ROTATION_ROLL_270_YAW_135, -SQRT_2, 0, -1);
47  TEST_ROTATION(ROTATION_PITCH_90, 1, 1, -1);
48  TEST_ROTATION(ROTATION_PITCH_270, -1, 1, 1);
49  TEST_ROTATION(ROTATION_PITCH_180_YAW_90, -1, -1, -1);
50  TEST_ROTATION(ROTATION_PITCH_180_YAW_270, 1, 1, -1);
51  TEST_ROTATION(ROTATION_ROLL_90_PITCH_90, 1, -1, -1);
52  TEST_ROTATION(ROTATION_ROLL_180_PITCH_90, -1, -1, -1);
53  TEST_ROTATION(ROTATION_ROLL_270_PITCH_90, -1, 1, -1);
54  TEST_ROTATION(ROTATION_ROLL_90_PITCH_180, -1, -1, -1);
55  TEST_ROTATION(ROTATION_ROLL_270_PITCH_180, -1, 1, 1);
56  TEST_ROTATION(ROTATION_ROLL_90_PITCH_270, -1, -1, 1);
57  TEST_ROTATION(ROTATION_ROLL_180_PITCH_270, 1, -1, 1);
58  TEST_ROTATION(ROTATION_ROLL_270_PITCH_270, 1, 1, 1);
59  TEST_ROTATION(ROTATION_ROLL_90_PITCH_180_YAW_90, 1, -1, -1);
60  TEST_ROTATION(ROTATION_ROLL_90_YAW_270, -1, -1, 1);
61  TEST_ROTATION(ROTATION_ROLL_90_PITCH_68_YAW_293, -0.4066309f, -1.5839677f, -0.5706992f);
62  TEST_ROTATION(ROTATION_PITCH_315, 0, 1, SQRT_2);
63  TEST_ROTATION(ROTATION_ROLL_90_PITCH_315, 0, -1, SQRT_2);
64 
65  EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
66 }
67 
68 TEST(MathTest, IsZero)
69 {
70  EXPECT_FALSE(is_zero(0.1));
71  EXPECT_FALSE(is_zero(0.0001));
72  EXPECT_TRUE(is_zero(0.f));
73  EXPECT_TRUE(is_zero(FLT_MIN));
74  EXPECT_TRUE(is_zero(-FLT_MIN));
75 }
76 
77 TEST(MathTest, IsEqual)
78 {
79  EXPECT_FALSE(is_equal(1, 0));
80  EXPECT_TRUE(is_equal(1, 1));
81  EXPECT_FALSE(is_equal(0.1, 0.10001));
82  EXPECT_FALSE(is_equal(0.1, -0.1001));
83  EXPECT_TRUE(is_equal(0.f, 0.0f));
84  EXPECT_FALSE(is_equal(1.f, 1.f + FLT_EPSILON));
85  EXPECT_TRUE(is_equal(1.f, 1.f + FLT_EPSILON / 2.f));
86  EXPECT_TRUE(is_equal(1.f, (float)(1.f - DBL_EPSILON)));
87 
88  // false because the common type is double
89  EXPECT_FALSE(is_equal(double(1.), 1 + 2 * std::numeric_limits<double>::epsilon()));
90 
91  // true because the common type is float
92  EXPECT_TRUE(is_equal(1.f, (float)(1. + std::numeric_limits<double>::epsilon())));
93 }
94 
95 TEST(MathTest, Square)
96 {
97  float sq_0 = sq(0);
98  float sq_1 = sq(1);
99  float sq_2 = sq(2);
100 
101  EXPECT_EQ(0.f, sq_0);
102  EXPECT_EQ(1.f, sq_1);
103  EXPECT_EQ(4.f, sq_2);
104 }
105 
106 TEST(MathTest, Norm)
107 {
108  float norm_1 = norm(1, 4.2);
109  float norm_2 = norm(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1);
110  float norm_3 = norm(0, 5.3);
111  float norm_4 = norm(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
112  float norm_5 = norm(3,4);
113  float norm_6 = norm(4,3,12);
114 
115  EXPECT_FLOAT_EQ(norm_1, 4.3174066f);
116  EXPECT_EQ(norm_2, 4.f);
117  EXPECT_EQ(norm_3, 5.3f);
118  EXPECT_EQ(norm_4, 0.f);
119  EXPECT_EQ(norm_5, 5.f);
120  EXPECT_EQ(norm_6, 13.f);
121 }
122 
123 
124 TEST(MathTest, Constrain)
125 {
126  for (int i = 0; i < 1000; i++) {
127  if (i < 250) {
128  EXPECT_EQ(250, constrain_float(i, 250, 500));
129  EXPECT_EQ(250, constrain_int16(i, 250, 500));
130  EXPECT_EQ(250, constrain_int32(i, 250, 500));
131  } else if (i > 500) {
132  EXPECT_EQ(500, constrain_float(i, 250, 500));
133  EXPECT_EQ(500, constrain_int16(i, 250, 500));
134  EXPECT_EQ(500, constrain_int32(i, 250, 500));
135  } else {
136  EXPECT_EQ(i, constrain_float(i, 250, 500));
137  EXPECT_EQ(i, constrain_int16(i, 250, 500));
138  EXPECT_EQ(i, constrain_int32(i, 250, 500));
139  }
140  }
141 
142  for (int i = 0; i <= 1000; i++) {
143  int c = i - 1000;
144  if (c < -250) {
145  EXPECT_EQ(-250, constrain_float(c, -250, -50));
146  EXPECT_EQ(-250, constrain_int16(c, -250, -50));
147  EXPECT_EQ(-250, constrain_int32(c, -250, -50));
148  } else if(c > -50) {
149  EXPECT_EQ(-50, constrain_float(c, -250, -50));
150  EXPECT_EQ(-50, constrain_int16(c, -250, -50));
151  EXPECT_EQ(-50, constrain_int32(c, -250, -50));
152  } else {
153  EXPECT_EQ(c, constrain_float(c, -250, -50));
154  EXPECT_EQ(c, constrain_int16(c, -250, -50));
155  EXPECT_EQ(c, constrain_int32(c, -250, -50));
156  }
157  }
158 
159  for (int i = 0; i <= 2000; i++) {
160  int c = i - 1000;
161  if (c < -250) {
162  EXPECT_EQ(-250, constrain_float(c, -250, 50));
163  EXPECT_EQ(-250, constrain_int16(c, -250, 50));
164  EXPECT_EQ(-250, constrain_int32(c, -250, 50));
165  } else if(c > 50) {
166  EXPECT_EQ(50, constrain_float(c, -250, 50));
167  EXPECT_EQ(50, constrain_int16(c, -250, 50));
168  EXPECT_EQ(50, constrain_int32(c, -250, 50));
169  } else {
170  EXPECT_EQ(c, constrain_float(c, -250, 50));
171  EXPECT_EQ(c, constrain_int16(c, -250, 50));
172  EXPECT_EQ(c, constrain_int32(c, -250, 50));
173  }
174  }
175 
176  EXPECT_EQ(20.0, constrain_value(20.0, 19.9, 20.1));
177  EXPECT_EQ(20.0, constrain_value(20.0f, 19.9f, 20.1f));
178 
179  EXPECT_EQ(19.9, constrain_value(19.9, 19.9, 20.1));
180  EXPECT_EQ(19.9f, constrain_value(19.9f, 19.9f, 20.1f));
181 
182  EXPECT_EQ(19.9, constrain_value(19.8, 19.9, 20.1));
183  EXPECT_EQ(19.9f, constrain_value(19.8f, 19.9f, 20.1f));
184 }
185 
186 TEST(MathWrapTest, Angle180)
187 {
188  // Full circle test
189  for (int32_t i = 0; i < 36000; i += 100) {
190  if (i < 18000) {
191  // smaller pole position
192  EXPECT_EQ(i, wrap_180_cd(i));
193  EXPECT_EQ(-i, wrap_180_cd(-i));
194  } else if (i == 18000) {
195  // hit pole position -180/+180 degree
196  EXPECT_EQ(i, wrap_180_cd(i));
197  EXPECT_EQ(i, wrap_180_cd(-i));
198  } else {
199  // bigger pole position
200  EXPECT_EQ(-(36000 - i), wrap_180_cd(i));
201  EXPECT_EQ(36000 - i, wrap_180_cd(-i));
202  }
203  }
204 
205  EXPECT_EQ(4500.f, wrap_180_cd(4500.f));
206  EXPECT_EQ(9000.f, wrap_180_cd(9000.f));
207  EXPECT_EQ(18000.f, wrap_180_cd(18000.f));
208  EXPECT_EQ(-17990.f, wrap_180_cd(18010.f));
209  EXPECT_EQ(-9000.f, wrap_180_cd(27000.f));
210  EXPECT_EQ(0.f, wrap_180_cd(36000.f));
211  EXPECT_EQ(0.f, wrap_180_cd(72000.f));
212  EXPECT_EQ(0.f, wrap_180_cd(360000.f));
213  EXPECT_EQ(0.f, wrap_180_cd(720000.f));
214  EXPECT_EQ(0.f, wrap_180_cd(-3600000000.f));
215 
216  EXPECT_EQ(-4500.f, wrap_180_cd(-4500.f));
217  EXPECT_EQ(-9000.f, wrap_180_cd(-9000.f));
218  EXPECT_EQ(18000.f, wrap_180_cd(-18000.f));
219  EXPECT_EQ(17990.f, wrap_180_cd(-18010.f));
220  EXPECT_EQ(9000.f, wrap_180_cd(-27000.f));
221  EXPECT_EQ(0.f, wrap_180_cd(-36000.f));
222  EXPECT_EQ(0.f, wrap_180_cd(-72000.f));
223 }
224 
225 TEST(MathWrapTest, Angle360)
226 {
227  // Full circle test
228  for (int32_t i = 0; i <= 36000; i += 100) {
229  if (i == 0) {
230  // hit pole position
231  EXPECT_EQ(i, wrap_360_cd(i));
232  EXPECT_EQ(i, wrap_360_cd(-i));
233  } else if (i < 36000) {
234  // between pole position
235  EXPECT_EQ(i, wrap_360_cd(i));
236  EXPECT_EQ(36000 - i, wrap_360_cd(-i));
237  } else if (i == 36000) {
238  // hit pole position
239  EXPECT_EQ(0, wrap_360_cd(i));
240  EXPECT_EQ(0, wrap_360_cd(-i));
241  }
242  }
243 
244  EXPECT_EQ(4500.f, wrap_360_cd(4500.f));
245  EXPECT_EQ(9000.f, wrap_360_cd(9000.f));
246  EXPECT_EQ(18000.f, wrap_360_cd(18000.f));
247  EXPECT_EQ(27000.f, wrap_360_cd(27000.f));
248  EXPECT_EQ(0.f, wrap_360_cd(36000.f));
249  EXPECT_EQ(0.f, wrap_360_cd(72000.f));
250  EXPECT_EQ(0.f, wrap_360_cd(360000.f));
251  EXPECT_EQ(0.f, wrap_360_cd(720000.f));
252  EXPECT_EQ( 0.f, wrap_360_cd(-3600000000.f));
253 
254  EXPECT_EQ(31500.f, wrap_360_cd(-4500.f));
255  EXPECT_EQ(27000.f, wrap_360_cd(-9000.f));
256  EXPECT_EQ(18000.f, wrap_360_cd(-18000.f));
257  EXPECT_EQ(9000.f, wrap_360_cd(-27000.f));
258  EXPECT_EQ(0.f, wrap_360_cd(-36000.f));
259  EXPECT_EQ(0.f, wrap_360_cd(-72000.f));
260 }
261 
262 TEST(MathWrapTest, AnglePI)
263 {
264  const float accuracy = 1.0e-5;
265 
266  EXPECT_NEAR(M_PI, wrap_PI(M_PI), accuracy);
267  EXPECT_NEAR(0.f, wrap_PI(M_2PI), accuracy);
268  EXPECT_NEAR(0, wrap_PI(M_PI * 10), accuracy);
269 }
270 
271 TEST(MathWrapTest, Angle2PI)
272 {
273  const float accuracy = 1.0e-5;
274 
275  EXPECT_NEAR(M_PI, wrap_2PI(M_PI), accuracy);
276  EXPECT_NEAR(0.f, wrap_2PI(M_2PI), accuracy);
277  EXPECT_NEAR(0.f, wrap_2PI(M_PI * 10), accuracy);
278  EXPECT_NEAR(0.f, wrap_2PI(0.f), accuracy);
279  EXPECT_NEAR(M_PI, wrap_2PI(-M_PI), accuracy);
280  EXPECT_NEAR(0, wrap_2PI(-M_2PI), accuracy);
281 }
282 
283 AP_GTEST_MAIN()
norm
float norm(const T first, const U second, const Params... parameters)
Definition: AP_Math.h:190
TEST_ROTATION
#define TEST_ROTATION(rotation, _x, _y, _z)
constrain_int16
int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
Definition: AP_Math.h:147
wrap_360_cd
auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f))
Definition: AP_Math.cpp:140
SQRT_2
#define SQRT_2
Definition: test_math.cpp:5
wrap_180_cd
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
Definition: AP_Math.cpp:111
wrap_PI
float wrap_PI(const T radian)
Definition: AP_Math.cpp:152
is_zero
bool is_zero(const T fVal1)
Definition: AP_Math.h:40
f
#define f(i)
constrain_int32
int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
Definition: AP_Math.h:152
TEST
TEST(VectorTest, Rotations)
Definition: test_math.cpp:7
M_2PI
#define M_2PI
Definition: definitions.h:19
AP_Math.h
constrain_float
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
wrap_2PI
float wrap_2PI(const T radian)
Definition: AP_Math.cpp:167
is_equal
std::enable_if< std::is_integral< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value,bool >::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
Definition: AP_Math.cpp:12
sq
float sq(const T val)
Definition: AP_Math.h:170
M_PI
#define M_PI
Definition: definitions.h:10
constrain_value
T constrain_value(const T amt, const T low, const T high)
Definition: AP_Math.cpp:182
Generated on Sun Jun 17 2018 14:18:50 for APM:Libraries by   doxygen 1.8.13