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 TEST(Angles, from_degrees)
77 {
78  double epsilon = 1e-9;
79  EXPECT_NEAR(0, from_degrees(0), epsilon);
80  EXPECT_NEAR(M_PI/2, from_degrees(90), epsilon);
81  EXPECT_NEAR(M_PI, from_degrees(180), epsilon);
82  EXPECT_NEAR(M_PI*3/2, from_degrees(270), epsilon);
83  EXPECT_NEAR(2*M_PI, from_degrees(360), epsilon);
84  EXPECT_NEAR(M_PI/3, from_degrees(60), epsilon);
85  EXPECT_NEAR(M_PI*2/3, from_degrees(120), epsilon);
86  EXPECT_NEAR(M_PI/4, from_degrees(45), epsilon);
87  EXPECT_NEAR(M_PI*3/4, from_degrees(135), epsilon);
88  EXPECT_NEAR(M_PI/6, from_degrees(30), epsilon);
89 
90 }
91 
92 TEST(Angles, to_degrees)
93 {
94  double epsilon = 1e-9;
95  EXPECT_NEAR(to_degrees(0), 0, epsilon);
96  EXPECT_NEAR(to_degrees(M_PI/2), 90, epsilon);
97  EXPECT_NEAR(to_degrees(M_PI), 180, epsilon);
98  EXPECT_NEAR(to_degrees(M_PI*3/2), 270, epsilon);
99  EXPECT_NEAR(to_degrees(2*M_PI), 360, epsilon);
100  EXPECT_NEAR(to_degrees(M_PI/3), 60, epsilon);
101  EXPECT_NEAR(to_degrees(M_PI*2/3), 120, epsilon);
102  EXPECT_NEAR(to_degrees(M_PI/4), 45, epsilon);
103  EXPECT_NEAR(to_degrees(M_PI*3/4), 135, epsilon);
104  EXPECT_NEAR(to_degrees(M_PI/6), 30, epsilon);
105 }
106 
107 TEST(Angles, normalize_angle_positive)
108 {
109  double epsilon = 1e-9;
110  EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
111  EXPECT_NEAR(M_PI, normalize_angle_positive(M_PI), epsilon);
112  EXPECT_NEAR(0, normalize_angle_positive(2*M_PI), epsilon);
113  EXPECT_NEAR(M_PI, normalize_angle_positive(3*M_PI), epsilon);
114  EXPECT_NEAR(0, normalize_angle_positive(4*M_PI), epsilon);
115 
116  EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
117  EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
118  EXPECT_NEAR(0, normalize_angle_positive(-2*M_PI), epsilon);
119  EXPECT_NEAR(M_PI, normalize_angle_positive(-3*M_PI), epsilon);
120  EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI), epsilon);
121 
122  EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
123  EXPECT_NEAR(3*M_PI/2, normalize_angle_positive(-M_PI/2), epsilon);
124  EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
125  EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
126  EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI/2), epsilon);
127 
128  EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
129  EXPECT_NEAR(M_PI/2, normalize_angle_positive(M_PI/2), epsilon);
130  EXPECT_NEAR(M_PI/2, normalize_angle_positive(5*M_PI/2), epsilon);
131  EXPECT_NEAR(M_PI/2, normalize_angle_positive(9*M_PI/2), epsilon);
132  EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
133 
134 }
135 
136 
137 TEST(Angles, normalize_angle)
138 {
139  double epsilon = 1e-9;
140  EXPECT_NEAR(0, normalize_angle(0), epsilon);
141  EXPECT_NEAR(M_PI, normalize_angle(M_PI), epsilon);
142  EXPECT_NEAR(0, normalize_angle(2*M_PI), epsilon);
143  EXPECT_NEAR(M_PI, normalize_angle(3*M_PI), epsilon);
144  EXPECT_NEAR(0, normalize_angle(4*M_PI), epsilon);
145 
146  EXPECT_NEAR(0, normalize_angle(-0), epsilon);
147  EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
148  EXPECT_NEAR(0, normalize_angle(-2*M_PI), epsilon);
149  EXPECT_NEAR(M_PI, normalize_angle(-3*M_PI), epsilon);
150  EXPECT_NEAR(0, normalize_angle(-4*M_PI), epsilon);
151 
152  EXPECT_NEAR(0, normalize_angle(-0), epsilon);
153  EXPECT_NEAR(-M_PI/2, normalize_angle(-M_PI/2), epsilon);
154  EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
155  EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
156  EXPECT_NEAR(0, normalize_angle(-4*M_PI/2), epsilon);
157 
158  EXPECT_NEAR(0, normalize_angle(0), epsilon);
159  EXPECT_NEAR(M_PI/2, normalize_angle(M_PI/2), epsilon);
160  EXPECT_NEAR(M_PI/2, normalize_angle(5*M_PI/2), epsilon);
161  EXPECT_NEAR(M_PI/2, normalize_angle(9*M_PI/2), epsilon);
162  EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
163 
164 }
165 
166 TEST(Angles, shortest_angular_distance)
167 {
168  double epsilon = 1e-9;
169  EXPECT_NEAR(M_PI/2, shortest_angular_distance(0, M_PI/2), epsilon);
170  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(0, -M_PI/2), epsilon);
171  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI/2, 0), epsilon);
172  EXPECT_NEAR(M_PI/2, shortest_angular_distance(-M_PI/2, 0), epsilon);
173 
174  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI, M_PI/2), epsilon);
175  EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI, -M_PI/2), epsilon);
176  EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI/2, M_PI), epsilon);
177  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-M_PI/2, M_PI), epsilon);
178 
179  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(5*M_PI, M_PI/2), epsilon);
180  EXPECT_NEAR(M_PI/2, shortest_angular_distance(7*M_PI, -M_PI/2), epsilon);
181  EXPECT_NEAR(M_PI/2, shortest_angular_distance(9*M_PI/2, M_PI), epsilon);
182  EXPECT_NEAR(M_PI/2, shortest_angular_distance(-3*M_PI/2, M_PI), epsilon);
183 
184  // Backside wrapping
185  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-3*M_PI/4, 3*M_PI/4), epsilon);
186  EXPECT_NEAR(M_PI/2, shortest_angular_distance(3*M_PI/4, -3*M_PI/4), epsilon);
187 }
188 
189 TEST(Angles, two_pi_complement)
190 {
191  double epsilon = 1e-9;
192  EXPECT_NEAR(two_pi_complement(0), 2*M_PI, epsilon);
193  EXPECT_NEAR(two_pi_complement(2*M_PI), 0, epsilon);
194  EXPECT_NEAR(two_pi_complement(-2*M_PI), 0, epsilon);
195  EXPECT_NEAR(two_pi_complement(2*M_PI-epsilon), -epsilon, epsilon);
196  EXPECT_NEAR(two_pi_complement(-2*M_PI+epsilon), epsilon, epsilon);
197  EXPECT_NEAR(two_pi_complement(M_PI/2), -3*M_PI/2, epsilon);
198  EXPECT_NEAR(two_pi_complement(M_PI), -M_PI, epsilon);
199  EXPECT_NEAR(two_pi_complement(-M_PI), M_PI, epsilon);
200  EXPECT_NEAR(two_pi_complement(-M_PI/2), 3*M_PI/2, epsilon);
201 
202  EXPECT_NEAR(two_pi_complement(3*M_PI), -M_PI, epsilon);
203  EXPECT_NEAR(two_pi_complement(-3.0*M_PI), M_PI, epsilon);
204  EXPECT_NEAR(two_pi_complement(-5.0*M_PI/2.0), 3*M_PI/2, epsilon);
205 
206 
207 
208 }
209 
210 TEST(Angles, find_min_max_delta)
211 {
212  double epsilon = 1e-9;
213  double min_delta, max_delta;
214  // Straight forward full range
215  EXPECT_TRUE(find_min_max_delta( 0, -M_PI, M_PI, min_delta, max_delta));
216  EXPECT_NEAR(min_delta, -M_PI, epsilon);
217  EXPECT_NEAR(max_delta, M_PI, epsilon);
218 
219  // M_PI/2 Full Range
220  EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, M_PI, min_delta, max_delta));
221  EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
222  EXPECT_NEAR(max_delta, M_PI/2, epsilon);
223 
224  // -M_PI/2 Full range
225  EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, M_PI, min_delta, max_delta));
226  EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
227  EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
228 
229  // Straight forward partial range
230  EXPECT_TRUE(find_min_max_delta( 0, -M_PI/2, M_PI/2, min_delta, max_delta));
231  EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
232  EXPECT_NEAR(max_delta, M_PI/2, epsilon);
233 
234  // M_PI/4 Partial Range
235  EXPECT_TRUE(find_min_max_delta( M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
236  EXPECT_NEAR(min_delta, -3*M_PI/4, epsilon);
237  EXPECT_NEAR(max_delta, M_PI/4, epsilon);
238 
239  // -M_PI/4 Partial Range
240  EXPECT_TRUE(find_min_max_delta( -M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
241  EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
242  EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
243 
244  // bump stop negative full range
245  EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
246  EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
247  EXPECT_NEAR(min_delta, 0.0, epsilon);
248  EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
249 
250  EXPECT_TRUE(find_min_max_delta(-0.25,0.25,-0.25,min_delta, max_delta));
251  EXPECT_NEAR(min_delta, -2*M_PI+0.5, epsilon);
252  EXPECT_NEAR(max_delta, 0.0, epsilon);
253 
254  // bump stop positive full range
255  EXPECT_TRUE(find_min_max_delta( M_PI-epsilon, -M_PI, M_PI, min_delta, max_delta));
256  //EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
257  EXPECT_NEAR(min_delta, -2*M_PI+epsilon, epsilon);
258  EXPECT_NEAR(max_delta, epsilon, epsilon);
259 
260  // bump stop negative partial range
261  EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
262  EXPECT_NEAR(min_delta, 0, epsilon);
263  EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
264 
265  // bump stop positive partial range
266  EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI/2, M_PI/2, min_delta, max_delta));
267  EXPECT_NEAR(min_delta, 0.0, epsilon);
268  EXPECT_NEAR(max_delta, M_PI, epsilon);
269 
270 
271  //Test out of range negative
272  EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
273  //Test out of range postive
274  EXPECT_FALSE(find_min_max_delta( M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
275 
276 
277  // M_PI/4 Partial Range
278  EXPECT_TRUE(find_min_max_delta( 3*M_PI/4, M_PI/2, -M_PI/2, min_delta, max_delta));
279  EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
280  EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
281 
282 
283 }
284 
285 int main(int argc, char **argv){
286  testing::InitGoogleTest(&argc, argv);
287  return RUN_ALL_TESTS();
288 }
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:215
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
shortest_angular_distance
Definition: angles.h:102
angles::normalize_angle
static double normalize_angle(double angle)
normalize
Definition: angles.h:81
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:285
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:116
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:140

angles
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 19:19:27