MathToolbox.cpp
Go to the documentation of this file.
00001 //
00002 // MathToolbox.cpp
00003 //
00004 //
00005 // Math functions.
00006 //
00008 
00009 #include "MathToolbox.hpp"
00010 #include <limits>
00011 
00012 
00013 const double NaN_double = std::numeric_limits<double>::quiet_NaN();
00014 //const float NaN = std::numeric_limits<float>::quiet_NaN();
00015 
00019 double hypot(double x, double y, double z)
00020 {
00021         return sqrt(x*x + y*y + z*z);
00022 }
00023 
00024 
00031 double normalizeRadians (double radians)
00032 {
00033         if (std::abs(radians) > PI)
00034         {
00035                 // For numerical stability we must use this sin/cos/atan2
00036                 // implementation even though it might consume more cycles.
00037                 // Note that radians = -pi converts to atan2(0,-1) = +pi!
00038                 radians = std::atan2 (std::sin(radians), std::cos(radians));
00039                 // radians now in (-pi,+pi]
00040         }
00041         if (radians == PI)      // equality of doubles ... just for the sake of completeness
00042         {
00043                 // Convert half-open interval from  (-pi,+pi]  to  [-pi,+pi)
00044                 radians = -PI;
00045         }
00046         return radians;
00047 }


libsick_ldmrs
Author(s): SICK AG , Martin G√ľnther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50