00001 #include "angles/angles.h"
00002 #include <gtest/gtest.h>
00003
00004 using namespace angles;
00005
00006 TEST(Angles, shortestDistanceWithLimits){
00007 double shortest_angle;
00008 bool result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25,shortest_angle);
00009 EXPECT_FALSE(result);
00010
00011 result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25,shortest_angle);
00012 EXPECT_FALSE(result);
00013
00014 result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25,shortest_angle);
00015 EXPECT_TRUE(result);
00016 EXPECT_NEAR(shortest_angle, -2*M_PI+1.0,1e-6);
00017
00018 result = angles::shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25,shortest_angle);
00019 EXPECT_TRUE(result);
00020 EXPECT_NEAR(shortest_angle, 0,1e-6);
00021
00022 result = angles::shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25,shortest_angle);
00023 EXPECT_FALSE(result);
00024 EXPECT_NEAR(shortest_angle, -0.5,1e-6);
00025
00026 result = angles::shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25,shortest_angle);
00027 EXPECT_FALSE(result);
00028 EXPECT_NEAR(shortest_angle, 0.5,1e-6);
00029
00030 result = angles::shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25,shortest_angle);
00031 EXPECT_FALSE(result);
00032 EXPECT_NEAR(shortest_angle, -2*M_PI+0.4,1e-6);
00033
00034 result = angles::shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25,shortest_angle);
00035 EXPECT_FALSE(result);
00036 EXPECT_NEAR(shortest_angle,2*M_PI-0.4,1e-6);
00037
00038 result = angles::shortest_angular_distance_with_limits(0.2,0,0.25,-0.25,shortest_angle);
00039 EXPECT_FALSE(result);
00040 EXPECT_NEAR(shortest_angle,2*M_PI-0.2,1e-6);
00041
00042 result = angles::shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25,shortest_angle);
00043 EXPECT_FALSE(result);
00044 EXPECT_NEAR(shortest_angle,-2*M_PI+0.2,1e-6);
00045
00046 result = angles::shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25,shortest_angle);
00047 EXPECT_TRUE(result);
00048 EXPECT_NEAR(shortest_angle,-0.25,1e-6);
00049
00050 result = angles::shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25,shortest_angle);
00051 EXPECT_TRUE(result);
00052 EXPECT_NEAR(shortest_angle,-2*M_PI+0.75,1e-6);
00053
00054 result = angles::shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25,shortest_angle);
00055 EXPECT_TRUE(result);
00056 EXPECT_NEAR(shortest_angle,-2*M_PI+0.5+0.2500001,1e-6);
00057
00058 result = angles::shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25,shortest_angle);
00059 EXPECT_FALSE(result);
00060
00061 result = angles::shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25,shortest_angle);
00062 EXPECT_FALSE(result);
00063
00064 result = angles::shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3,shortest_angle);
00065 EXPECT_FALSE(result);
00066
00067 result = angles::shortest_angular_distance_with_limits(-0.6, M_PI*3.0/4.0,-0.25,0.3,shortest_angle);
00068 EXPECT_FALSE(result);
00069
00070 result = angles::shortest_angular_distance_with_limits(-M_PI, M_PI,-M_PI,M_PI,shortest_angle);
00071 EXPECT_TRUE(result);
00072 EXPECT_NEAR(shortest_angle,0.0,1e-6);
00073
00074 }
00075
00076 TEST(Angles, from_degrees)
00077 {
00078 double epsilon = 1e-9;
00079 EXPECT_NEAR(0, from_degrees(0), epsilon);
00080 EXPECT_NEAR(M_PI/2, from_degrees(90), epsilon);
00081 EXPECT_NEAR(M_PI, from_degrees(180), epsilon);
00082 EXPECT_NEAR(M_PI*3/2, from_degrees(270), epsilon);
00083 EXPECT_NEAR(2*M_PI, from_degrees(360), epsilon);
00084 EXPECT_NEAR(M_PI/3, from_degrees(60), epsilon);
00085 EXPECT_NEAR(M_PI*2/3, from_degrees(120), epsilon);
00086 EXPECT_NEAR(M_PI/4, from_degrees(45), epsilon);
00087 EXPECT_NEAR(M_PI*3/4, from_degrees(135), epsilon);
00088 EXPECT_NEAR(M_PI/6, from_degrees(30), epsilon);
00089
00090 }
00091
00092 TEST(Angles, to_degrees)
00093 {
00094 double epsilon = 1e-9;
00095 EXPECT_NEAR(to_degrees(0), 0, epsilon);
00096 EXPECT_NEAR(to_degrees(M_PI/2), 90, epsilon);
00097 EXPECT_NEAR(to_degrees(M_PI), 180, epsilon);
00098 EXPECT_NEAR(to_degrees(M_PI*3/2), 270, epsilon);
00099 EXPECT_NEAR(to_degrees(2*M_PI), 360, epsilon);
00100 EXPECT_NEAR(to_degrees(M_PI/3), 60, epsilon);
00101 EXPECT_NEAR(to_degrees(M_PI*2/3), 120, epsilon);
00102 EXPECT_NEAR(to_degrees(M_PI/4), 45, epsilon);
00103 EXPECT_NEAR(to_degrees(M_PI*3/4), 135, epsilon);
00104 EXPECT_NEAR(to_degrees(M_PI/6), 30, epsilon);
00105 }
00106
00107 TEST(Angles, normalize_angle_positive)
00108 {
00109 double epsilon = 1e-9;
00110 EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
00111 EXPECT_NEAR(M_PI, normalize_angle_positive(M_PI), epsilon);
00112 EXPECT_NEAR(0, normalize_angle_positive(2*M_PI), epsilon);
00113 EXPECT_NEAR(M_PI, normalize_angle_positive(3*M_PI), epsilon);
00114 EXPECT_NEAR(0, normalize_angle_positive(4*M_PI), epsilon);
00115
00116 EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
00117 EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
00118 EXPECT_NEAR(0, normalize_angle_positive(-2*M_PI), epsilon);
00119 EXPECT_NEAR(M_PI, normalize_angle_positive(-3*M_PI), epsilon);
00120 EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI), epsilon);
00121
00122 EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
00123 EXPECT_NEAR(3*M_PI/2, normalize_angle_positive(-M_PI/2), epsilon);
00124 EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
00125 EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
00126 EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI/2), epsilon);
00127
00128 EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
00129 EXPECT_NEAR(M_PI/2, normalize_angle_positive(M_PI/2), epsilon);
00130 EXPECT_NEAR(M_PI/2, normalize_angle_positive(5*M_PI/2), epsilon);
00131 EXPECT_NEAR(M_PI/2, normalize_angle_positive(9*M_PI/2), epsilon);
00132 EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
00133
00134 }
00135
00136
00137 TEST(Angles, normalize_angle)
00138 {
00139 double epsilon = 1e-9;
00140 EXPECT_NEAR(0, normalize_angle(0), epsilon);
00141 EXPECT_NEAR(M_PI, normalize_angle(M_PI), epsilon);
00142 EXPECT_NEAR(0, normalize_angle(2*M_PI), epsilon);
00143 EXPECT_NEAR(M_PI, normalize_angle(3*M_PI), epsilon);
00144 EXPECT_NEAR(0, normalize_angle(4*M_PI), epsilon);
00145
00146 EXPECT_NEAR(0, normalize_angle(-0), epsilon);
00147 EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
00148 EXPECT_NEAR(0, normalize_angle(-2*M_PI), epsilon);
00149 EXPECT_NEAR(M_PI, normalize_angle(-3*M_PI), epsilon);
00150 EXPECT_NEAR(0, normalize_angle(-4*M_PI), epsilon);
00151
00152 EXPECT_NEAR(0, normalize_angle(-0), epsilon);
00153 EXPECT_NEAR(-M_PI/2, normalize_angle(-M_PI/2), epsilon);
00154 EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
00155 EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
00156 EXPECT_NEAR(0, normalize_angle(-4*M_PI/2), epsilon);
00157
00158 EXPECT_NEAR(0, normalize_angle(0), epsilon);
00159 EXPECT_NEAR(M_PI/2, normalize_angle(M_PI/2), epsilon);
00160 EXPECT_NEAR(M_PI/2, normalize_angle(5*M_PI/2), epsilon);
00161 EXPECT_NEAR(M_PI/2, normalize_angle(9*M_PI/2), epsilon);
00162 EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
00163
00164 }
00165
00166 TEST(Angles, shortest_angular_distance)
00167 {
00168 double epsilon = 1e-9;
00169 EXPECT_NEAR(M_PI/2, shortest_angular_distance(0, M_PI/2), epsilon);
00170 EXPECT_NEAR(-M_PI/2, shortest_angular_distance(0, -M_PI/2), epsilon);
00171 EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI/2, 0), epsilon);
00172 EXPECT_NEAR(M_PI/2, shortest_angular_distance(-M_PI/2, 0), epsilon);
00173
00174 EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI, M_PI/2), epsilon);
00175 EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI, -M_PI/2), epsilon);
00176 EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI/2, M_PI), epsilon);
00177 EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-M_PI/2, M_PI), epsilon);
00178
00179 EXPECT_NEAR(-M_PI/2, shortest_angular_distance(5*M_PI, M_PI/2), epsilon);
00180 EXPECT_NEAR(M_PI/2, shortest_angular_distance(7*M_PI, -M_PI/2), epsilon);
00181 EXPECT_NEAR(M_PI/2, shortest_angular_distance(9*M_PI/2, M_PI), epsilon);
00182 EXPECT_NEAR(M_PI/2, shortest_angular_distance(-3*M_PI/2, M_PI), epsilon);
00183
00184
00185 EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-3*M_PI/4, 3*M_PI/4), epsilon);
00186 EXPECT_NEAR(M_PI/2, shortest_angular_distance(3*M_PI/4, -3*M_PI/4), epsilon);
00187 }
00188
00189 TEST(Angles, two_pi_complement)
00190 {
00191 double epsilon = 1e-9;
00192 EXPECT_NEAR(two_pi_complement(0), 2*M_PI, epsilon);
00193 EXPECT_NEAR(two_pi_complement(2*M_PI), 0, epsilon);
00194 EXPECT_NEAR(two_pi_complement(-2*M_PI), 0, epsilon);
00195 EXPECT_NEAR(two_pi_complement(2*M_PI-epsilon), -epsilon, epsilon);
00196 EXPECT_NEAR(two_pi_complement(-2*M_PI+epsilon), epsilon, epsilon);
00197 EXPECT_NEAR(two_pi_complement(M_PI/2), -3*M_PI/2, epsilon);
00198 EXPECT_NEAR(two_pi_complement(M_PI), -M_PI, epsilon);
00199 EXPECT_NEAR(two_pi_complement(-M_PI), M_PI, epsilon);
00200 EXPECT_NEAR(two_pi_complement(-M_PI/2), 3*M_PI/2, epsilon);
00201
00202 EXPECT_NEAR(two_pi_complement(3*M_PI), -M_PI, epsilon);
00203 EXPECT_NEAR(two_pi_complement(-3.0*M_PI), M_PI, epsilon);
00204 EXPECT_NEAR(two_pi_complement(-5.0*M_PI/2.0), 3*M_PI/2, epsilon);
00205
00206
00207
00208 }
00209
00210 TEST(Angles, find_min_max_delta)
00211 {
00212 double epsilon = 1e-9;
00213 double min_delta, max_delta;
00214
00215 EXPECT_TRUE(find_min_max_delta( 0, -M_PI, M_PI, min_delta, max_delta));
00216 EXPECT_NEAR(min_delta, -M_PI, epsilon);
00217 EXPECT_NEAR(max_delta, M_PI, epsilon);
00218
00219
00220 EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, M_PI, min_delta, max_delta));
00221 EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
00222 EXPECT_NEAR(max_delta, M_PI/2, epsilon);
00223
00224
00225 EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, M_PI, min_delta, max_delta));
00226 EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
00227 EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
00228
00229
00230 EXPECT_TRUE(find_min_max_delta( 0, -M_PI/2, M_PI/2, min_delta, max_delta));
00231 EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
00232 EXPECT_NEAR(max_delta, M_PI/2, epsilon);
00233
00234
00235 EXPECT_TRUE(find_min_max_delta( M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
00236 EXPECT_NEAR(min_delta, -3*M_PI/4, epsilon);
00237 EXPECT_NEAR(max_delta, M_PI/4, epsilon);
00238
00239
00240 EXPECT_TRUE(find_min_max_delta( -M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
00241 EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
00242 EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
00243
00244
00245 EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
00246 EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
00247 EXPECT_NEAR(min_delta, 0.0, epsilon);
00248 EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
00249
00250 EXPECT_TRUE(find_min_max_delta(-0.25,0.25,-0.25,min_delta, max_delta));
00251 EXPECT_NEAR(min_delta, -2*M_PI+0.5, epsilon);
00252 EXPECT_NEAR(max_delta, 0.0, epsilon);
00253
00254
00255 EXPECT_TRUE(find_min_max_delta( M_PI-epsilon, -M_PI, M_PI, min_delta, max_delta));
00256
00257 EXPECT_NEAR(min_delta, -2*M_PI+epsilon, epsilon);
00258 EXPECT_NEAR(max_delta, epsilon, epsilon);
00259
00260
00261 EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
00262 EXPECT_NEAR(min_delta, 0, epsilon);
00263 EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
00264
00265
00266 EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI/2, M_PI/2, min_delta, max_delta));
00267 EXPECT_NEAR(min_delta, 0.0, epsilon);
00268 EXPECT_NEAR(max_delta, M_PI, epsilon);
00269
00270
00271
00272 EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
00273
00274 EXPECT_FALSE(find_min_max_delta( M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
00275
00276
00277
00278 EXPECT_TRUE(find_min_max_delta( 3*M_PI/4, M_PI/2, -M_PI/2, min_delta, max_delta));
00279 EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
00280 EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
00281
00282
00283 }
00284
00285 int main(int argc, char **argv){
00286 testing::InitGoogleTest(&argc, argv);
00287 return RUN_ALL_TESTS();
00288 }