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 }
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:289
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
shortest_angular_distance
Definition: angles.h:103
angles::normalize_angle
static double normalize_angle(double angle)
normalize
Definition: angles.h:83
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:222
angles
Definition: angles.h:41
angles::normalize_angle_positive
static double normalize_angle_positive(double angle)
normalize_angle_positive
Definition: angles.h:68
angles::from_degrees
static double from_degrees(double degrees)
Convert degrees to radians.
Definition: angles.h:48
main
int main(int argc, char **argv)
Definition: utest.cpp:320
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:117
angles::to_degrees
static double to_degrees(double radians)
Convert radians to degrees.
Definition: angles.h:56
TEST
TEST(Angles, shortestDistanceWithLimits)
Definition: utest.cpp:6
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. If you do use it, read the documentation very carefully. Returns the min and max amount (in radians) that can be moved from "from" angle to "left_limit" and "right_limit".
Definition: angles.h:141

angles
Author(s): John Hsu
autogenerated on Fri Mar 13 2020 03:55:59