KatanaNativeInterface $VERSION$

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 #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     // converting all parameters to _angleT (usually =double)
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     // converting all parameters to _angleT (usually = double)
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