00001 00027 #include <RobotUtil.H> 00028 #include <cmath> 00029 00030 namespace WURDE{ 00031 double normalizeAngle(double angle){ 00032 while(angle>=M_PI){ 00033 angle-=2*M_PI; 00034 } 00035 00036 while(angle<-M_PI){ 00037 angle+=2*M_PI; 00038 } 00039 00040 return angle; 00041 } 00042 }