Go to the documentation of this file.00001 #ifndef CONVENIENCE_MATH_FUNCTIONS_MATHFUNCTIONS_H
00002 #define CONVENIENCE_MATH_FUNCTIONS_MATHFUNCTIONS_H
00003
00004 #include <eigen_conversions/eigen_msg.h>
00005 #include <fstream>
00006
00007 namespace convenience_math_functions
00008 {
00009
00015 class MathFunctions
00016 {
00017
00018 public:
00021 MathFunctions() {}
00022
00023 ~MathFunctions()
00024 {
00025 }
00026
00032 static double capToPI(const double value);
00033
00034 template<typename Flt>
00035 static void capToPI(std::vector<Flt>& v)
00036 {
00037 for (typename std::vector<Flt>::iterator it = v.begin(); it != v.end(); ++it)
00038 {
00039 *it = capToPI(*it);
00040 }
00041 }
00042
00051 static double limitsToTwoPI(const double value, const double lowLimit, const double highLimit);
00052
00057 static double angleDistance(const double _f1, const double _f2);
00058
00059 static double quatAngularDistance(const geometry_msgs::Quaternion& _q1, const geometry_msgs::Quaternion& _q2);
00060
00061 static double quatAngularDistance(const Eigen::Quaterniond& _q1, const Eigen::Quaterniond& _q2);
00062
00063 static double vecAngularDistance(const Eigen::Vector3d& _v1, const Eigen::Vector3d& _v2);
00064
00065 static Eigen::Quaterniond getRotationFromTo(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2);
00066
00067 static bool equalFlt(float first, float second, float tolerance);
00071 static bool equalFloats(const std::vector<float>& first, const std::vector<float>& second, float tolerance);
00072 };
00073 }
00074 #endif // CONVENIENCE_MATH_FUNCTIONS_MATHFUNCTIONS_H