Go to the documentation of this file.00001
00011
00012
00013
00014
00015 #include <cmath>
00016 #include "../../include/ecl/geometry/angle.hpp"
00017 #include <ecl/math/constants.hpp>
00018
00019
00020
00021
00022
00023 namespace ecl {
00024
00025
00026
00027
00028
00029 const float& wrap_angle(float &angle) {
00030 if ( ( angle <= pi ) && ( angle >= -pi ) ) {
00031 return angle;
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;
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 }