ecl_geometry: angle.cpp Source File

Go to the documentation of this file.
00001 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <cmath>
00016 #include "../../include/ecl/geometry/angle.hpp"
00017 #include <ecl/math/constants.hpp>
00018 
00019 /*****************************************************************************
00020 ** Namespaces
00021 *****************************************************************************/
00022 
00023 namespace ecl {
00024 
00025 /*****************************************************************************
00026 ** Implementation [Functions]
00027 *****************************************************************************/
00028 
00029 const float& wrap_angle(float &angle) {
00030         if ( (  angle <= pi ) && ( angle >= -pi ) ) {
00031                 return angle; // nothing to do.
00032         }
00033         if ( angle < 0.0 ) {
00034                 angle = fmodf(angle-pi,2.0*pi)+pi;
00035         } else {
00036                 angle = fmodf(angle+pi,2.0*pi)-pi;
00037         }
00038         return angle;
00039 }
00040 float wrap_angle(const float &angle) {
00041         float wrapped;
00042         if ( (  angle <= pi ) && ( angle >= -pi ) ) {
00043                 wrapped = angle;
00044         } else if ( angle < 0.0 ) {
00045                 wrapped = fmodf(angle-pi,2.0*pi)+pi;
00046         } else {
00047                 wrapped = fmodf(angle+pi,2.0*pi)-pi;
00048         }
00049         return wrapped;
00050 }
00051 
00052 const double& wrap_angle(double &angle) {
00053         if ( (  angle <= pi ) && ( angle >= -pi ) ) {
00054                 return angle; // nothing to do.
00055         }
00056         if ( angle < 0.0 ) {
00057                 angle = fmod(angle-pi,2.0*pi)+pi;
00058         } else {
00059                 angle = fmod(angle+pi,2.0*pi)-pi;
00060         }
00061         return angle;
00062 }
00063 
00064 double wrap_angle(const double &angle) {
00065         double wrapped;
00066         if ( (  angle <= pi ) && ( angle >= -pi ) ) {
00067                 wrapped = angle;
00068         } else if ( angle < 0.0 ) {
00069                 wrapped = fmod(angle-pi,2.0*pi)+pi;
00070         } else {
00071                 wrapped = fmod(angle+pi,2.0*pi)-pi;
00072         }
00073         return wrapped;
00074 }
00075 
00076 } // namespace ecl