utest.cpp
Go to the documentation of this file.
1 #include "angles/angles.h"
2 #include <gtest/gtest.h>
3 
4 using namespace angles;
5 
6 TEST(Angles, shortestDistanceWithLimits){
7  double shortest_angle;
8  bool result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25,shortest_angle);
9  EXPECT_FALSE(result);
10 
11  result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25,shortest_angle);
12  EXPECT_FALSE(result);
13 
14  result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25,shortest_angle);
15  EXPECT_TRUE(result);
16  EXPECT_NEAR(shortest_angle, -2*M_PI+1.0,1e-6);
17 
18  result = angles::shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25,shortest_angle);
19  EXPECT_TRUE(result);
20  EXPECT_NEAR(shortest_angle, 0,1e-6);
21 
22  result = angles::shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25,shortest_angle);
23  EXPECT_FALSE(result);
24  EXPECT_NEAR(shortest_angle, -0.5,1e-6);
25 
26  result = angles::shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25,shortest_angle);
27  EXPECT_FALSE(result);
28  EXPECT_NEAR(shortest_angle, 0.5,1e-6);
29 
30  result = angles::shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25,shortest_angle);
31  EXPECT_FALSE(result);
32  EXPECT_NEAR(shortest_angle, -2*M_PI+0.4,1e-6);
33 
34  result = angles::shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25,shortest_angle);
35  EXPECT_FALSE(result);
36  EXPECT_NEAR(shortest_angle,2*M_PI-0.4,1e-6);
37 
38  result = angles::shortest_angular_distance_with_limits(0.2,0,0.25,-0.25,shortest_angle);
39  EXPECT_FALSE(result);
40  EXPECT_NEAR(shortest_angle,2*M_PI-0.2,1e-6);
41 
42  result = angles::shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25,shortest_angle);
43  EXPECT_FALSE(result);
44  EXPECT_NEAR(shortest_angle,-2*M_PI+0.2,1e-6);
45 
46  result = angles::shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25,shortest_angle);
47  EXPECT_TRUE(result);
48  EXPECT_NEAR(shortest_angle,-0.25,1e-6);
49 
50  result = angles::shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25,shortest_angle);
51  EXPECT_TRUE(result);
52  EXPECT_NEAR(shortest_angle,-2*M_PI+0.75,1e-6);
53 
54  result = angles::shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25,shortest_angle);
55  EXPECT_TRUE(result);
56  EXPECT_NEAR(shortest_angle,-2*M_PI+0.5+0.2500001,1e-6);
57 
58  result = angles::shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25,shortest_angle);
59  EXPECT_FALSE(result);
60 
61  result = angles::shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25,shortest_angle);
62  EXPECT_FALSE(result);
63 
64  result = angles::shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3,shortest_angle);
65  EXPECT_FALSE(result);
66 
67  result = angles::shortest_angular_distance_with_limits(-0.6, M_PI*3.0/4.0,-0.25,0.3,shortest_angle);
68  EXPECT_FALSE(result);
69 
70  result = angles::shortest_angular_distance_with_limits(-M_PI, M_PI,-M_PI,M_PI,shortest_angle);
71  EXPECT_TRUE(result);
72  EXPECT_NEAR(shortest_angle,0.0,1e-6);
73 
74 }
75 
76 
77 TEST(Angles, shortestDistanceWithLargeLimits)
78 {
79  double shortest_angle;
80  bool result;
81 
82  // 'delta' is valid
83  result = angles::shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI, shortest_angle);
84  EXPECT_TRUE(result);
85  EXPECT_NEAR(shortest_angle, 0.5*M_PI, 1e-6);
86 
87  // 'delta' is not valid, but 'delta_2pi' is
88  result = angles::shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI, shortest_angle);
89  EXPECT_TRUE(result);
90  EXPECT_NEAR(shortest_angle, -1.5*M_PI, 1e-6);
91 
92  // neither 'delta' nor 'delta_2pi' are valid
93  result = angles::shortest_angular_distance_with_large_limits(2*M_PI, M_PI, 2*M_PI-0.1, 2*M_PI+0.1, shortest_angle);
94  EXPECT_FALSE(result);
95 
96  // start position outside limits
97  result = angles::shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI, shortest_angle);
98  EXPECT_FALSE(result);
99 
100  // invalid limits (lower > upper)
101  result = angles::shortest_angular_distance_with_large_limits(0, 0.1, 2*M_PI, -2*M_PI, shortest_angle);
102  EXPECT_FALSE(result);
103 
104  // specific test case
105  result = angles::shortest_angular_distance_with_large_limits(0.999507, 1.0, -20*M_PI, 20*M_PI, shortest_angle);
106  EXPECT_TRUE(result);
107  EXPECT_NEAR(shortest_angle, 0.000493, 1e-6);
108 }
109 
110 
111 TEST(Angles, from_degrees)
112 {
113  double epsilon = 1e-9;
114  EXPECT_NEAR(0, from_degrees(0), epsilon);
115  EXPECT_NEAR(M_PI/2, from_degrees(90), epsilon);
116  EXPECT_NEAR(M_PI, from_degrees(180), epsilon);
117  EXPECT_NEAR(M_PI*3/2, from_degrees(270), epsilon);
118  EXPECT_NEAR(2*M_PI, from_degrees(360), epsilon);
119  EXPECT_NEAR(M_PI/3, from_degrees(60), epsilon);
120  EXPECT_NEAR(M_PI*2/3, from_degrees(120), epsilon);
121  EXPECT_NEAR(M_PI/4, from_degrees(45), epsilon);
122  EXPECT_NEAR(M_PI*3/4, from_degrees(135), epsilon);
123  EXPECT_NEAR(M_PI/6, from_degrees(30), epsilon);
124 
125 }
126 
127 TEST(Angles, to_degrees)
128 {
129  double epsilon = 1e-9;
130  EXPECT_NEAR(to_degrees(0), 0, epsilon);
131  EXPECT_NEAR(to_degrees(M_PI/2), 90, epsilon);
132  EXPECT_NEAR(to_degrees(M_PI), 180, epsilon);
133  EXPECT_NEAR(to_degrees(M_PI*3/2), 270, epsilon);
134  EXPECT_NEAR(to_degrees(2*M_PI), 360, epsilon);
135  EXPECT_NEAR(to_degrees(M_PI/3), 60, epsilon);
136  EXPECT_NEAR(to_degrees(M_PI*2/3), 120, epsilon);
137  EXPECT_NEAR(to_degrees(M_PI/4), 45, epsilon);
138  EXPECT_NEAR(to_degrees(M_PI*3/4), 135, epsilon);
139  EXPECT_NEAR(to_degrees(M_PI/6), 30, epsilon);
140 }
141 
142 TEST(Angles, normalize_angle_positive)
143 {
144  double epsilon = 1e-9;
145  EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
146  EXPECT_NEAR(M_PI, normalize_angle_positive(M_PI), epsilon);
147  EXPECT_NEAR(0, normalize_angle_positive(2*M_PI), epsilon);
148  EXPECT_NEAR(M_PI, normalize_angle_positive(3*M_PI), epsilon);
149  EXPECT_NEAR(0, normalize_angle_positive(4*M_PI), epsilon);
150 
151  EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
152  EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
153  EXPECT_NEAR(0, normalize_angle_positive(-2*M_PI), epsilon);
154  EXPECT_NEAR(M_PI, normalize_angle_positive(-3*M_PI), epsilon);
155  EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI), epsilon);
156 
157  EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
158  EXPECT_NEAR(3*M_PI/2, normalize_angle_positive(-M_PI/2), epsilon);
159  EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
160  EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
161  EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI/2), epsilon);
162 
163  EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
164  EXPECT_NEAR(M_PI/2, normalize_angle_positive(M_PI/2), epsilon);
165  EXPECT_NEAR(M_PI/2, normalize_angle_positive(5*M_PI/2), epsilon);
166  EXPECT_NEAR(M_PI/2, normalize_angle_positive(9*M_PI/2), epsilon);
167  EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
168 
169 }
170 
171 
172 TEST(Angles, normalize_angle)
173 {
174  double epsilon = 1e-9;
175  EXPECT_NEAR(0, normalize_angle(0), epsilon);
176  EXPECT_NEAR(M_PI, normalize_angle(M_PI), epsilon);
177  EXPECT_NEAR(0, normalize_angle(2*M_PI), epsilon);
178  EXPECT_NEAR(M_PI, normalize_angle(3*M_PI), epsilon);
179  EXPECT_NEAR(0, normalize_angle(4*M_PI), epsilon);
180 
181  EXPECT_NEAR(0, normalize_angle(-0), epsilon);
182  EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
183  EXPECT_NEAR(0, normalize_angle(-2*M_PI), epsilon);
184  EXPECT_NEAR(M_PI, normalize_angle(-3*M_PI), epsilon);
185  EXPECT_NEAR(0, normalize_angle(-4*M_PI), epsilon);
186 
187  EXPECT_NEAR(0, normalize_angle(-0), epsilon);
188  EXPECT_NEAR(-M_PI/2, normalize_angle(-M_PI/2), epsilon);
189  EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
190  EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
191  EXPECT_NEAR(0, normalize_angle(-4*M_PI/2), epsilon);
192 
193  EXPECT_NEAR(0, normalize_angle(0), epsilon);
194  EXPECT_NEAR(M_PI/2, normalize_angle(M_PI/2), epsilon);
195  EXPECT_NEAR(M_PI/2, normalize_angle(5*M_PI/2), epsilon);
196  EXPECT_NEAR(M_PI/2, normalize_angle(9*M_PI/2), epsilon);
197  EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
198 
199 }
200 
201 TEST(Angles, shortest_angular_distance)
202 {
203  double epsilon = 1e-9;
204  EXPECT_NEAR(M_PI/2, shortest_angular_distance(0, M_PI/2), epsilon);
205  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(0, -M_PI/2), epsilon);
206  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI/2, 0), epsilon);
207  EXPECT_NEAR(M_PI/2, shortest_angular_distance(-M_PI/2, 0), epsilon);
208 
209  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI, M_PI/2), epsilon);
210  EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI, -M_PI/2), epsilon);
211  EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI/2, M_PI), epsilon);
212  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-M_PI/2, M_PI), epsilon);
213 
214  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(5*M_PI, M_PI/2), epsilon);
215  EXPECT_NEAR(M_PI/2, shortest_angular_distance(7*M_PI, -M_PI/2), epsilon);
216  EXPECT_NEAR(M_PI/2, shortest_angular_distance(9*M_PI/2, M_PI), epsilon);
217  EXPECT_NEAR(M_PI/2, shortest_angular_distance(-3*M_PI/2, M_PI), epsilon);
218 
219  // Backside wrapping
220  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-3*M_PI/4, 3*M_PI/4), epsilon);
221  EXPECT_NEAR(M_PI/2, shortest_angular_distance(3*M_PI/4, -3*M_PI/4), epsilon);
222 }
223 
224 TEST(Angles, two_pi_complement)
225 {
226  double epsilon = 1e-9;
227  EXPECT_NEAR(two_pi_complement(0), 2*M_PI, epsilon);
228  EXPECT_NEAR(two_pi_complement(2*M_PI), 0, epsilon);
229  EXPECT_NEAR(two_pi_complement(-2*M_PI), 0, epsilon);
230  EXPECT_NEAR(two_pi_complement(2*M_PI-epsilon), -epsilon, epsilon);
231  EXPECT_NEAR(two_pi_complement(-2*M_PI+epsilon), epsilon, epsilon);
232  EXPECT_NEAR(two_pi_complement(M_PI/2), -3*M_PI/2, epsilon);
233  EXPECT_NEAR(two_pi_complement(M_PI), -M_PI, epsilon);
234  EXPECT_NEAR(two_pi_complement(-M_PI), M_PI, epsilon);
235  EXPECT_NEAR(two_pi_complement(-M_PI/2), 3*M_PI/2, epsilon);
236 
237  EXPECT_NEAR(two_pi_complement(3*M_PI), -M_PI, epsilon);
238  EXPECT_NEAR(two_pi_complement(-3.0*M_PI), M_PI, epsilon);
239  EXPECT_NEAR(two_pi_complement(-5.0*M_PI/2.0), 3*M_PI/2, epsilon);
240 
241 
242 
243 }
244 
245 TEST(Angles, find_min_max_delta)
246 {
247  double epsilon = 1e-9;
248  double min_delta, max_delta;
249  // Straight forward full range
250  EXPECT_TRUE(find_min_max_delta( 0, -M_PI, M_PI, min_delta, max_delta));
251  EXPECT_NEAR(min_delta, -M_PI, epsilon);
252  EXPECT_NEAR(max_delta, M_PI, epsilon);
253 
254  // M_PI/2 Full Range
255  EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, M_PI, min_delta, max_delta));
256  EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
257  EXPECT_NEAR(max_delta, M_PI/2, epsilon);
258 
259  // -M_PI/2 Full range
260  EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, M_PI, min_delta, max_delta));
261  EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
262  EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
263 
264  // Straight forward partial range
265  EXPECT_TRUE(find_min_max_delta( 0, -M_PI/2, M_PI/2, min_delta, max_delta));
266  EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
267  EXPECT_NEAR(max_delta, M_PI/2, epsilon);
268 
269  // M_PI/4 Partial Range
270  EXPECT_TRUE(find_min_max_delta( M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
271  EXPECT_NEAR(min_delta, -3*M_PI/4, epsilon);
272  EXPECT_NEAR(max_delta, M_PI/4, epsilon);
273 
274  // -M_PI/4 Partial Range
275  EXPECT_TRUE(find_min_max_delta( -M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
276  EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
277  EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
278 
279  // bump stop negative full range
280  EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
281  EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
282  EXPECT_NEAR(min_delta, 0.0, epsilon);
283  EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
284 
285  EXPECT_TRUE(find_min_max_delta(-0.25,0.25,-0.25,min_delta, max_delta));
286  EXPECT_NEAR(min_delta, -2*M_PI+0.5, epsilon);
287  EXPECT_NEAR(max_delta, 0.0, epsilon);
288 
289  // bump stop positive full range
290  EXPECT_TRUE(find_min_max_delta( M_PI-epsilon, -M_PI, M_PI, min_delta, max_delta));
291  //EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
292  EXPECT_NEAR(min_delta, -2*M_PI+epsilon, epsilon);
293  EXPECT_NEAR(max_delta, epsilon, epsilon);
294 
295  // bump stop negative partial range
296  EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
297  EXPECT_NEAR(min_delta, 0, epsilon);
298  EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
299 
300  // bump stop positive partial range
301  EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI/2, M_PI/2, min_delta, max_delta));
302  EXPECT_NEAR(min_delta, 0.0, epsilon);
303  EXPECT_NEAR(max_delta, M_PI, epsilon);
304 
305 
306  //Test out of range negative
307  EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
308  //Test out of range postive
309  EXPECT_FALSE(find_min_max_delta( M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
310 
311 
312  // M_PI/4 Partial Range
313  EXPECT_TRUE(find_min_max_delta( 3*M_PI/4, M_PI/2, -M_PI/2, min_delta, max_delta));
314  EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
315  EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
316 
317 
318 }
319 
320 int main(int argc, char **argv){
321  testing::InitGoogleTest(&argc, argv);
322  return RUN_ALL_TESTS();
323 }
main
int main(int argc, char **argv)
Definition: utest.cpp:320
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
shortest_angular_distance
Definition: angles.h:135
angles::normalize_angle
static double normalize_angle(double angle)
normalize
Definition: angles.h:115
angles
Definition: angles.h:41
angles::from_degrees
static double from_degrees(double degrees)
Convert degrees to radians.
Definition: angles.h:80
angles::shortest_angular_distance_with_limits
static bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by...
Definition: angles.h:321
angles::normalize_angle_positive
static double normalize_angle_positive(double angle)
normalize_angle_positive
Definition: angles.h:100
angles::find_min_max_delta
static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
This function is only intended for internal use and not intended for external use....
Definition: angles.h:173
angles.h
TEST
TEST(Angles, shortestDistanceWithLimits)
Definition: utest.cpp:6
angles::to_degrees
static double to_degrees(double radians)
Convert radians to degrees.
Definition: angles.h:88
angles::shortest_angular_distance_with_large_limits
static bool shortest_angular_distance_with_large_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
Returns the delta from from_angle to to_angle, making sure it does not violate limits specified by le...
Definition: angles.h:254
angles::two_pi_complement
static double two_pi_complement(double angle)
returns the angle in [-2*M_PI, 2*M_PI] going the other way along the unit circle.
Definition: angles.h:149

angles
Author(s): John Hsu , Tully Foote
autogenerated on Tue Mar 1 2022 23:45:04