turtlebot_panorama: geometry.h Source File

Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009  ** Ifdefs
00010  *****************************************************************************/
00011 
00012 #ifndef TURTLEBOT_PANORAMA_GEOMETRY_H_
00013 #define TURTLEBOT_PANORAMA_GEOMETRY_H_
00014 
00015 /*****************************************************************************
00016  ** Includes
00017  *****************************************************************************/
00018 
00019 #include <cmath>
00020 
00021 /*****************************************************************************
00022  ** Namespaces
00023  *****************************************************************************/
00024 
00025 namespace turtlebot_panorama
00026 {
00027 
00028 /*****************************************************************************
00029  ** Interfaces
00030  *****************************************************************************/
00031 
00032 template<typename T>
00033   T degrees_to_radians(const T &degrees)
00034   {
00035     static const double degs_to_rads = M_PI / 180.0;
00036     return degrees * degs_to_rads;
00037   }
00038 
00039 template<typename T>
00040   T radians_to_degrees(const T &radians)
00041   {
00042     static const double rads_to_degs = 180.0 / M_PI;
00043     return radians * rads_to_degs;
00044   }
00045 
00046 template<typename T>
00047   T wrap_angle(const T &angle)
00048   {
00049     float wrapped;
00050     if ((angle <= M_PI) && (angle >= -M_PI))
00051     {
00052       wrapped = angle;
00053     }
00054     else if (angle < 0.0)
00055     {
00056       wrapped = fmodf(angle - M_PI, 2.0 * M_PI) + M_PI;
00057     }
00058     else
00059     {
00060       wrapped = fmodf(angle + M_PI, 2.0 * M_PI) - M_PI;
00061     }
00062     return wrapped;
00063   }
00064 
00065 } // namespace turtlebot_panorama
00066 
00067 #endif /* TURTLEBOT_PANORAMA_GEOMETRY_H_ */