angles: angles: utest.cpp Source 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 }
00185 
00186 TEST(Angles, two_pi_complement)
00187 {
00188   double epsilon = 1e-9;
00189   EXPECT_NEAR(two_pi_complement(0), 2*M_PI, epsilon);
00190   EXPECT_NEAR(two_pi_complement(2*M_PI), 0, epsilon); 
00191   EXPECT_NEAR(two_pi_complement(-2*M_PI), 0, epsilon); 
00192   EXPECT_NEAR(two_pi_complement(2*M_PI-epsilon), -epsilon, epsilon); 
00193   EXPECT_NEAR(two_pi_complement(-2*M_PI+epsilon), epsilon, epsilon); 
00194   EXPECT_NEAR(two_pi_complement(M_PI/2), -3*M_PI/2, epsilon);
00195   EXPECT_NEAR(two_pi_complement(M_PI), -M_PI, epsilon);
00196   EXPECT_NEAR(two_pi_complement(-M_PI), M_PI, epsilon);
00197   EXPECT_NEAR(two_pi_complement(-M_PI/2), 3*M_PI/2, epsilon);
00198 
00199   EXPECT_NEAR(two_pi_complement(3*M_PI), -M_PI, epsilon); 
00200   EXPECT_NEAR(two_pi_complement(-3.0*M_PI), M_PI, epsilon);
00201   EXPECT_NEAR(two_pi_complement(-5.0*M_PI/2.0), 3*M_PI/2, epsilon);
00202 
00203 
00204 
00205 }
00206 
00207 TEST(Angles, find_min_max_delta)
00208 {
00209   double epsilon = 1e-9;
00210   double min_delta, max_delta;
00211   // Straight forward full range
00212   EXPECT_TRUE(find_min_max_delta( 0, -M_PI, M_PI, min_delta, max_delta));
00213   EXPECT_NEAR(min_delta, -M_PI, epsilon);
00214   EXPECT_NEAR(max_delta, M_PI, epsilon);
00215 
00216   // M_PI/2 Full Range
00217   EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, M_PI, min_delta, max_delta));
00218   EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
00219   EXPECT_NEAR(max_delta, M_PI/2, epsilon);
00220 
00221   // -M_PI/2 Full range
00222   EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, M_PI, min_delta, max_delta));
00223   EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
00224   EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
00225 
00226   // Straight forward partial range
00227   EXPECT_TRUE(find_min_max_delta( 0, -M_PI/2, M_PI/2, min_delta, max_delta));
00228   EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
00229   EXPECT_NEAR(max_delta, M_PI/2, epsilon);
00230   
00231   // M_PI/4 Partial Range
00232   EXPECT_TRUE(find_min_max_delta( M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
00233   EXPECT_NEAR(min_delta, -3*M_PI/4, epsilon);
00234   EXPECT_NEAR(max_delta, M_PI/4, epsilon);
00235 
00236   // -M_PI/4 Partial Range
00237   EXPECT_TRUE(find_min_max_delta( -M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
00238   EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
00239   EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
00240 
00241   // bump stop negative full range
00242   EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
00243   EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon)); 
00244   EXPECT_NEAR(min_delta, 0.0, epsilon);
00245   EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
00246 
00247   EXPECT_TRUE(find_min_max_delta(-0.25,0.25,-0.25,min_delta, max_delta));
00248   EXPECT_NEAR(min_delta, -2*M_PI+0.5, epsilon);
00249   EXPECT_NEAR(max_delta, 0.0, epsilon);
00250   
00251   // bump stop positive full range
00252   EXPECT_TRUE(find_min_max_delta( M_PI-epsilon, -M_PI, M_PI, min_delta, max_delta));
00253   //EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon)); 
00254   EXPECT_NEAR(min_delta, -2*M_PI+epsilon, epsilon);
00255   EXPECT_NEAR(max_delta, epsilon, epsilon);
00256 
00257   // bump stop negative partial range
00258   EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
00259   EXPECT_NEAR(min_delta, 0, epsilon);
00260   EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
00261 
00262   // bump stop positive partial range
00263   EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI/2, M_PI/2, min_delta, max_delta));
00264   EXPECT_NEAR(min_delta, 0.0, epsilon);
00265   EXPECT_NEAR(max_delta, M_PI, epsilon);
00266     
00267 
00268   //Test out of range negative
00269   EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
00270   //Test out of range postive
00271   EXPECT_FALSE(find_min_max_delta( M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
00272 
00273 
00274   // M_PI/4 Partial Range
00275   EXPECT_TRUE(find_min_max_delta( 3*M_PI/4, M_PI/2, -M_PI/2, min_delta, max_delta));
00276   EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
00277   EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
00278 
00279 
00280 }
00281 
00282 int main(int argc, char **argv){
00283   testing::InitGoogleTest(&argc, argv);
00284   return RUN_ALL_TESTS();
00285 }