angles: utest.cpp Source File

Go to the documentation of this file.
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   // Backside wrapping
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   // Straight forward full range
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   // M_PI/2 Full Range
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   // -M_PI/2 Full range
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   // Straight forward partial range
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   // M_PI/4 Partial Range
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   // -M_PI/4 Partial Range
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   // bump stop negative full range
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   // bump stop positive full range
00255   EXPECT_TRUE(find_min_max_delta( M_PI-epsilon, -M_PI, M_PI, min_delta, max_delta));
00256   //EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
00257   EXPECT_NEAR(min_delta, -2*M_PI+epsilon, epsilon);
00258   EXPECT_NEAR(max_delta, epsilon, epsilon);
00259 
00260   // bump stop negative partial range
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   // bump stop positive partial range
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   //Test out of range negative
00272   EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
00273   //Test out of range postive
00274   EXPECT_FALSE(find_min_max_delta( M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
00275 
00276 
00277   // M_PI/4 Partial Range
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 }