KatanaNativeInterface
$VERSION$
|
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