KatanaNativeInterface $VERSION$
MathHelperFunctions.h
Go to the documentation of this file.
1/***************************************************************************
2 * Copyright (C) 2006 by Tiziano Mueller *
3 * tiziano.mueller@neuronics.ch *
4 * *
5 * This program is free software; you can redistribute it and/or modify *
6 * it under the terms of the GNU General Public License as published by *
7 * the Free Software Foundation; either version 2 of the License, or *
8 * (at your option) any later version. *
9 * *
10 * This program is distributed in the hope that it will be useful, *
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13 * GNU General Public License for more details. *
14 * *
15 * You should have received a copy of the GNU General Public License *
16 * along with this program; if not, write to the *
17 * Free Software Foundation, Inc., *
18 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19 ***************************************************************************/
20
21#define M_PI 3.14159265358979323846
22
23#include <cmath>
24#include <vector>
25#include <functional>
26#include <cassert>
27
28#ifndef KNI_MATH_HELPER_FUNCTIONS
29#define KNI_MATH_HELPER_FUNCTIONS
30
31
32
33namespace KNI_MHF {
34
35//*****************************************************************
36
37template<typename _T> inline short sign(_T x) { return ( (x<0) ? -1 : 1 ); }
38
39//*****************************************************************
40
44template<typename _T> struct unary_precalc_sin : public std::unary_function<_T, _T> {
45 _T operator() (_T &x) {
46 return sin(x);
47 }
48};
49
53template<typename _T> struct unary_precalc_cos : public std::unary_function<_T, _T> {
54 _T operator() (_T x) {
55 return cos(x);
56 }
57};
58
59
60
61//*****************************************************************
62template<typename _T> inline _T atan1(_T in1, _T in2) {
63
64 if(in1==0.0)
65 return M_PI+sign(in2)*M_PI/2;
66
67 if(in1<0.0)
68 return atan(in2/in1)+M_PI;
69
70 if( (in1>0.0) && (in2<0.0) )
71 return atan(in2/in1)+2.0*M_PI;
72
73 return atan(in2/in1);
74}
75
76//*****************************************************************
77template<typename _T> inline _T acotan(const _T in) {
78 if(in == 0.0)
79 return M_PI/2;
80 else
81 return atan(1/in);
82}
83
84//*************************************************
85template<typename _T> inline _T atan0(const _T in1, const _T in2) {
86 if(in1 == 0.0)
87 return M_PI/2;
88 return atan(in2/in1);
89}
90
91//*************************************************
92template<typename _T> inline _T pow2(const _T in) {
93 return pow(in,2);
94}
95
96
100template<typename _T> inline _T rad2deg(const _T a) {
101 return a*(180.0/M_PI);
102}
103
107template<typename _T> struct unary_rad2deg : public std::unary_function<_T, _T> {
108 _T operator() (const _T a) { return rad2deg(a); }
109};
110
114template<typename _T> inline _T deg2rad(const _T a) {
115 return a*(M_PI/180.0);
116}
117
121template<typename _T> struct unary_deg2rad : public std::unary_function<_T, _T> {
122 _T operator() (const _T a) { deg2rad(a); }
123};
124
125//*************************************************
126template<typename _T> _T inline anglereduce(const _T a) {
127 return a - floor( a/(2*M_PI) )*2*M_PI;
128}
129//*************************************************
130
134template<typename _angleT, typename _encT> inline _encT rad2enc(_angleT const& angle, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
135 // converting all parameters to _angleT (usually =double)
136 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
137#ifdef WIN32
138 double temp = _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI);
139 return static_cast<_encT>( (temp >= 0) ? floor(temp + 0.5) : floor(temp - 0.5) );
140#else
141 return static_cast<_encT>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI) ) );
142#endif
143}
144
148template<typename _angleT, typename _encT> inline _angleT enc2rad(_encT const& enc, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
149 // converting all parameters to _angleT (usually = double)
150 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
151 return _angleOffset - (_enc - _encOffset)*2.0*M_PI/(_epc*_rotDir);
152}
153
157inline double findFirstEqualAngle(double cosValue, double sinValue, double tolerance) {
158 double v1[2], v2[2];
159
160 v1[0] = acos(cosValue);
161 v1[1] = -v1[0];
162 v2[0] = asin(sinValue);
163 v2[1] = M_PI - v2[0];
164
165 for(int i=0;i<2;++i) {
166 for(int j=0;j<2;++j) {
167 if(std::abs(anglereduce(v1[i]) - anglereduce(v2[j])) < tolerance) return v1[i];
168 }
169 }
170 assert(!"precondition for findFirstEqualAngle failed -> no equal angles found");
171 return 0;
172}
173
174
175}
176
177
178
179#endif
#define M_PI
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
converts encoders to absolute angles in radian
_T rad2deg(const _T a)
conversion from radian to degree
_T atan1(_T in1, _T in2)
_T atan0(const _T in1, const _T in2)
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
converts absolute angles in radian to encoders.
short sign(_T x)
_T anglereduce(const _T a)
_T deg2rad(const _T a)
conversion from degree to radian
_T pow2(const _T in)
_T acotan(const _T in)
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
Find the first equal angle.
a function-object version of rad2deg
function-object which calculates sinus for n-elements of a container if used together with a STL algo...
a function-object version of rad2deg