MathHelperFunctions.h
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2006-2008 by Neuronics AG                               *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00006 #define  MHF_PI      3.14159265358979323846
00007 
00008 #include <cmath>
00009 #include <vector>
00010 #include <functional>
00011 #include "exception.h"
00012 
00013 #ifndef MATH_HELPER_FUNCTIONS
00014 #define MATH_HELPER_FUNCTIONS
00015 
00016 
00017 
00018 namespace MHF {
00019 
00020 #ifdef WIN32
00021 template<typename _T> inline double round(_T x)
00022 // Copyright (C) 2001 Tor M. Aamodt, University of Toronto
00023 // Permisssion to use for all purposes commercial and otherwise granted.
00024 // THIS MATERIAL IS PROVIDED "AS IS" WITHOUT WARRANTY, OR ANY CONDITION OR
00025 // OTHER TERM OF ANY KIND INCLUDING, WITHOUT LIMITATION, ANY WARRANTY
00026 // OF MERCHANTABILITY, SATISFACTORY QUALITY, OR FITNESS FOR A PARTICULAR
00027 // PURPOSE.
00028 {
00029    if( x > 0 ) {
00030        __int64 xint = (__int64) (x+0.5);
00031        if( xint % 2 ) {
00032            // then we might have an even number...
00033            double diff = x - (double)xint;
00034            if( diff == -0.5 )
00035                return double(xint-1);
00036        }
00037        return double(xint);
00038    } else {
00039        __int64 xint = (__int64) (x-0.5);
00040        if( xint % 2 ) {
00041            // then we might have an even number...
00042            double diff = x - (double)xint;
00043            if( diff == 0.5 )
00044                return double(xint+1);
00045        }
00046        return double(xint);
00047    }
00048 }
00049 #endif // ifdef WIN32
00050 //*****************************************************************
00051 
00052 template<typename _T> inline short sign(_T x) { return ( (x<0) ? -1 : 1 ); }
00053 
00054 //*****************************************************************
00055 
00059 template<typename _T> struct unary_precalc_sin : public std::unary_function<_T, _T> {
00060     _T operator() (_T &x) {
00061         return sin(x);
00062     }
00063 };
00064 
00068 template<typename _T> struct unary_precalc_cos : public std::unary_function<_T, _T> {
00069     _T operator() (_T x) {
00070         return cos(x);
00071     }
00072 };
00073 
00074 
00075 
00076 //*****************************************************************
00077 template<typename _T> inline _T atan1(_T in1, _T in2) {
00078 
00079     if(in1==0.0)
00080         return MHF_PI+sign(in2)*MHF_PI/2;
00081 
00082     if(in1<0.0)
00083         return atan(in2/in1)+MHF_PI;
00084 
00085     if( (in1>0.0) && (in2<0.0) )
00086         return atan(in2/in1)+2.0*MHF_PI;
00087 
00088     return atan(in2/in1);
00089 }
00090 
00091 //*****************************************************************
00092 template<typename _T> inline _T acotan(const _T in) {
00093     if(in == 0.0)
00094         return MHF_PI/2;
00095     else
00096         return atan(1/in);
00097 }
00098 
00099 //*************************************************
00100 template<typename _T> inline _T atan0(const _T in1, const _T in2) {
00101     if(in1 == 0.0)
00102         return MHF_PI/2;
00103     return atan(in2/in1);
00104 }
00105 
00106 //*************************************************
00107 template<typename _T> inline _T pow2(const _T in) {
00108   return pow(in,2);
00109 }
00110 
00111 
00115 template<typename _T> inline _T rad2deg(const _T a) {
00116     return a*(180.0/MHF_PI);
00117 }
00118 
00122 template<typename _T> struct unary_rad2deg : public std::unary_function<_T, _T> {
00123     _T operator() (const _T a) { return rad2deg(a); }
00124 };
00125 
00129 template<typename _T> inline _T deg2rad(const _T a) {
00130     return a*(MHF_PI/180.0);
00131 }
00132 
00136 template<typename _T> struct unary_deg2rad : public std::unary_function<_T, _T> {
00137   _T operator() (const _T a) { deg2rad(a); }
00138 };
00139 
00140 //*************************************************
00141 template<typename _T> _T inline anglereduce(const _T a) {
00142     return a - floor( a/(2*MHF_PI) )*2*MHF_PI;
00143 }
00144 //*************************************************
00145 
00149 template<typename _angleT, typename _encT> inline _encT rad2enc(_angleT const& angle, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
00150     // converting all parameters to _angleT (usually =double)
00151     _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
00152     return static_cast<_encT>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*MHF_PI) ) );
00153 }
00154 
00158 template<typename _angleT, typename _encT> inline _angleT enc2rad(_encT const& enc, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
00159     // converting all parameters to _angleT (usually = double)
00160     _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
00161     return _angleOffset -  (_enc - _encOffset)*2.0*MHF_PI/(_epc*_rotDir);
00162 }
00163 
00167 inline double findFirstEqualAngle(double cosValue, double sinValue, double tolerance) {
00168     double v1[2], v2[2];
00169 
00170                 v1[0] = acos(cosValue);
00171                 v1[1] = -v1[0];
00172                 v2[0] = asin(sinValue);
00173                 v2[1] = MHF_PI - v2[0];
00174 
00175     for(int i=0;i<2;++i) {
00176                   for(int j=0;j<2;++j) {
00177                         if(std::abs(anglereduce(v1[i]) - anglereduce(v2[j])) < tolerance) return v1[i];
00178                   }
00179     }
00180         throw AnaGuess::Exception("precondition for findFirstEqualAngle failed -> no equal angles found", -2);
00181     return 0;
00182 }
00183 
00184 
00185 } // namespace
00186 
00187 
00188 
00189 #endif // ifndef MATH_HELPER_FUNCTIONS


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:33