MathHelperFunctions.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #define M_PI 3.14159265358979323846
00022
00023 #include <cmath>
00024 #include <vector>
00025 #include <functional>
00026 #include <cassert>
00027
00028 #ifndef KNI_MATH_HELPER_FUNCTIONS
00029 #define KNI_MATH_HELPER_FUNCTIONS
00030
00031
00032
00033 namespace KNI_MHF {
00034
00035
00036
00037 template<typename _T> inline short sign(_T x) { return ( (x<0) ? -1 : 1 ); }
00038
00039
00040
00044 template<typename _T> struct unary_precalc_sin : public std::unary_function<_T, _T> {
00045 _T operator() (_T &x) {
00046 return sin(x);
00047 }
00048 };
00049
00053 template<typename _T> struct unary_precalc_cos : public std::unary_function<_T, _T> {
00054 _T operator() (_T x) {
00055 return cos(x);
00056 }
00057 };
00058
00059
00060
00061
00062 template<typename _T> inline _T atan1(_T in1, _T in2) {
00063
00064 if(in1==0.0)
00065 return M_PI+sign(in2)*M_PI/2;
00066
00067 if(in1<0.0)
00068 return atan(in2/in1)+M_PI;
00069
00070 if( (in1>0.0) && (in2<0.0) )
00071 return atan(in2/in1)+2.0*M_PI;
00072
00073 return atan(in2/in1);
00074 }
00075
00076
00077 template<typename _T> inline _T acotan(const _T in) {
00078 if(in == 0.0)
00079 return M_PI/2;
00080 else
00081 return atan(1/in);
00082 }
00083
00084
00085 template<typename _T> inline _T atan0(const _T in1, const _T in2) {
00086 if(in1 == 0.0)
00087 return M_PI/2;
00088 return atan(in2/in1);
00089 }
00090
00091
00092 template<typename _T> inline _T pow2(const _T in) {
00093 return pow(in,2);
00094 }
00095
00096
00100 template<typename _T> inline _T rad2deg(const _T a) {
00101 return a*(180.0/M_PI);
00102 }
00103
00107 template<typename _T> struct unary_rad2deg : public std::unary_function<_T, _T> {
00108 _T operator() (const _T a) { return rad2deg(a); }
00109 };
00110
00114 template<typename _T> inline _T deg2rad(const _T a) {
00115 return a*(M_PI/180.0);
00116 }
00117
00121 template<typename _T> struct unary_deg2rad : public std::unary_function<_T, _T> {
00122 _T operator() (const _T a) { deg2rad(a); }
00123 };
00124
00125
00126 template<typename _T> _T inline anglereduce(const _T a) {
00127 return a - floor( a/(2*M_PI) )*2*M_PI;
00128 }
00129
00130
00134 template<typename _angleT, typename _encT> inline _encT rad2enc(_angleT const& angle, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
00135
00136 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
00137 #ifdef WIN32
00138 double temp = _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI);
00139 return static_cast<_encT>( (temp >= 0) ? floor(temp + 0.5) : floor(temp - 0.5) );
00140 #else
00141 return static_cast<_encT>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI) ) );
00142 #endif
00143 }
00144
00148 template<typename _angleT, typename _encT> inline _angleT enc2rad(_encT const& enc, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
00149
00150 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
00151 return _angleOffset - (_enc - _encOffset)*2.0*M_PI/(_epc*_rotDir);
00152 }
00153
00157 inline double findFirstEqualAngle(double cosValue, double sinValue, double tolerance) {
00158 double v1[2], v2[2];
00159
00160 v1[0] = acos(cosValue);
00161 v1[1] = -v1[0];
00162 v2[0] = asin(sinValue);
00163 v2[1] = M_PI - v2[0];
00164
00165 for(int i=0;i<2;++i) {
00166 for(int j=0;j<2;++j) {
00167 if(std::abs(anglereduce(v1[i]) - anglereduce(v2[j])) < tolerance) return v1[i];
00168 }
00169 }
00170 assert(!"precondition for findFirstEqualAngle failed -> no equal angles found");
00171 return 0;
00172 }
00173
00174
00175 }
00176
00177
00178
00179 #endif