Go to the documentation of this file.00001 #ifndef HELPERS_H
00002 #define HELPERS_H
00003
00004 static double normalize(double z)
00005 {
00006 return atan2(sin(z),cos(z));
00007 }
00008
00009 static double angle_diff(double a, double b)
00010 {
00011 double d1, d2;
00012 a = normalize(a);
00013 b = normalize(b);
00014 d1 = a-b;
00015 d2 = 2*M_PI - fabs(d1);
00016 if(d1 > 0)
00017 d2 *= -1.0;
00018 if(fabs(d1) < fabs(d2))
00019 return(d1);
00020 else
00021 return(d2);
00022 }
00023
00024 #endif // HELPERS_H