MathHelperFunctions.h
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2006 by Tiziano Mueller   *
00003  *   tiziano.mueller@neuronics.ch   *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 
00021 #define  M_PI      3.14159265358979323846
00022 
00023 //<tfromm date="22.05.2009">
00024 #include <cstdlib>
00025 //</tfromm>
00026 #include <cmath>
00027 #include <vector>
00028 #include <functional>
00029 #include <cassert>
00030 
00031 #ifndef KNI_MATH_HELPER_FUNCTIONS
00032 #define KNI_MATH_HELPER_FUNCTIONS
00033 
00034 
00035 
00036 namespace KNI_MHF {
00037 
00038 //*****************************************************************
00039 
00040 template<typename _T> inline short sign(_T x) { return ( (x<0) ? -1 : 1 ); }
00041 
00042 //*****************************************************************
00043 
00047 template<typename _T> struct unary_precalc_sin : public std::unary_function<_T, _T> {
00048     _T operator() (_T &x) {
00049         return sin(x);
00050     }
00051 };
00052 
00056 template<typename _T> struct unary_precalc_cos : public std::unary_function<_T, _T> {
00057     _T operator() (_T x) {
00058         return cos(x);
00059     }
00060 };
00061 
00062 
00063 
00064 //*****************************************************************
00065 template<typename _T> inline _T atan1(_T in1, _T in2) {
00066   
00067     if(in1==0.0 && in2 != 0.0)
00068             //FIXME: Verify -/+
00069         return M_PI-sign(in2)*M_PI/2;
00070         //return M_PI+sign(in2)*M_PI/2;
00071     
00072     if(in1==0.0 && in2 == 0.0)
00073             return 0.0;
00074     
00075     if(in1<0.0)
00076         return atan(in2/in1)+M_PI;
00077     
00078     if( (in1>0.0) && (in2<0.0) )
00079         return atan(in2/in1)+2.0*M_PI;
00080     
00081     return atan(in2/in1);
00082 }
00083 
00084 //*****************************************************************
00085 template<typename _T> inline _T acotan(const _T in) {
00086     if(in == 0.0)
00087         return M_PI/2;
00088     else
00089         return atan(1/in);
00090 }
00091 
00092 //*************************************************
00093 template<typename _T> inline _T atan0(const _T in1, const _T in2) {
00094     if(in1 == 0.0)
00095         return M_PI/2;
00096     return atan(in2/in1);
00097 }
00098 
00099 //*************************************************
00100 template<typename _T> inline _T pow2(const _T in) {
00101   return pow(in,2);
00102 }
00103 
00104 
00108 template<typename _T> inline _T rad2deg(const _T a) {
00109     return a*(180.0/M_PI);
00110 }
00111 
00115 template<typename _T> struct unary_rad2deg : public std::unary_function<_T, _T> {
00116     _T operator() (const _T a) { return rad2deg(a); }
00117 };
00118 
00122 template<typename _T> inline _T deg2rad(const _T a) {
00123     return a*(M_PI/180.0);
00124 }
00125 
00129 template<typename _T> struct unary_deg2rad : public std::unary_function<_T, _T> {
00130   _T operator() (const _T a) { deg2rad(a); }
00131 };
00132 
00133 //*************************************************
00134 template<typename _T> _T inline anglereduce(const _T a) {
00135     return a - floor( a/(2*M_PI) )*2*M_PI;
00136 }
00137 //*************************************************
00138 
00142 template<typename _angleT, typename _encT> inline _encT rad2enc(_angleT const& angle, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
00143     // converting all parameters to _angleT (usually =double)
00144     _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
00145 #ifdef WIN32
00146     double temp = _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI);
00147     return static_cast<_encT>( (temp >= 0) ? floor(temp + 0.5) : floor(temp - 0.5)  );
00148 #else
00149     return static_cast<_encT>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI) ) );
00150 #endif
00151 }
00152 
00156 template<typename _angleT, typename _encT> inline _angleT enc2rad(_encT const& enc, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
00157     // converting all parameters to _angleT (usually = double)
00158     _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
00159     return _angleOffset -  (_enc - _encOffset)*2.0*M_PI/(_epc*_rotDir);
00160 }
00161 
00165 inline double findFirstEqualAngle(double cosValue, double sinValue, double tolerance) {
00166     double v1[2], v2[2];
00167     
00168                 v1[0] = acos(cosValue);
00169                 v1[1] = -v1[0];
00170                 v2[0] = asin(sinValue);
00171                 v2[1] = M_PI - v2[0];
00172                 
00173     for(int i=0;i<2;++i) {
00174                   for(int j=0;j<2;++j) {
00175                         if(std::abs(anglereduce(v1[i]) - anglereduce(v2[j])) < tolerance) return v1[i]; 
00176                   }
00177     }
00178     assert(!"precondition for findFirstEqualAngle failed -> no equal angles found");
00179     return 0;
00180 }
00181 
00182 
00183 }
00184 
00185 
00186 
00187 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:54