include/common/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 //<tfromm date="22.05.2009">
24 #include <cstdlib>
25 //</tfromm>
26 #include <cmath>
27 #include <vector>
28 #include <functional>
29 #include <cassert>
30 
31 #ifndef KNI_MATH_HELPER_FUNCTIONS
32 #define KNI_MATH_HELPER_FUNCTIONS
33 
34 
35 
36 namespace KNI_MHF {
37 
38 //*****************************************************************
39 
40 template<typename _T> inline short sign(_T x) { return ( (x<0) ? -1 : 1 ); }
41 
42 //*****************************************************************
43 
47 template<typename _T> struct unary_precalc_sin : public std::unary_function<_T, _T> {
48  _T operator() (_T &x) {
49  return sin(x);
50  }
51 };
52 
56 template<typename _T> struct unary_precalc_cos : public std::unary_function<_T, _T> {
57  _T operator() (_T x) {
58  return cos(x);
59  }
60 };
61 
62 
63 
64 //*****************************************************************
65 template<typename _T> inline _T atan1(_T in1, _T in2) {
66 
67  if(in1==0.0 && in2 != 0.0)
68  //FIXME: Verify -/+
69  return M_PI-sign(in2)*M_PI/2;
70  //return M_PI+sign(in2)*M_PI/2;
71 
72  if(in1==0.0 && in2 == 0.0)
73  return 0.0;
74 
75  if(in1<0.0)
76  return atan(in2/in1)+M_PI;
77 
78  if( (in1>0.0) && (in2<0.0) )
79  return atan(in2/in1)+2.0*M_PI;
80 
81  return atan(in2/in1);
82 }
83 
84 //*****************************************************************
85 template<typename _T> inline _T acotan(const _T in) {
86  if(in == 0.0)
87  return M_PI/2;
88  else
89  return atan(1/in);
90 }
91 
92 //*************************************************
93 template<typename _T> inline _T atan0(const _T in1, const _T in2) {
94  if(in1 == 0.0)
95  return M_PI/2;
96  return atan(in2/in1);
97 }
98 
99 //*************************************************
100 template<typename _T> inline _T pow2(const _T in) {
101  return pow(in,2);
102 }
103 
104 
108 template<typename _T> inline _T rad2deg(const _T a) {
109  return a*(180.0/M_PI);
110 }
111 
115 template<typename _T> struct unary_rad2deg : public std::unary_function<_T, _T> {
116  _T operator() (const _T a) { return rad2deg(a); }
117 };
118 
122 template<typename _T> inline _T deg2rad(const _T a) {
123  return a*(M_PI/180.0);
124 }
125 
129 template<typename _T> struct unary_deg2rad : public std::unary_function<_T, _T> {
130  _T operator() (const _T a) { deg2rad(a); }
131 };
132 
133 //*************************************************
134 template<typename _T> _T inline anglereduce(const _T a) {
135  return a - floor( a/(2*M_PI) )*2*M_PI;
136 }
137 //*************************************************
138 
142 template<typename _angleT, typename _encT> inline _encT rad2enc(_angleT const& angle, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
143  // converting all parameters to _angleT (usually =double)
144  _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
145 #ifdef WIN32
146  double temp = _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI);
147  return static_cast<_encT>( (temp >= 0) ? floor(temp + 0.5) : floor(temp - 0.5) );
148 #else
149  return static_cast<_encT>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*M_PI) ) );
150 #endif
151 }
152 
156 template<typename _angleT, typename _encT> inline _angleT enc2rad(_encT const& enc, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
157  // converting all parameters to _angleT (usually = double)
158  _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
159  return _angleOffset - (_enc - _encOffset)*2.0*M_PI/(_epc*_rotDir);
160 }
161 
165 inline double findFirstEqualAngle(double cosValue, double sinValue, double tolerance) {
166  double v1[2], v2[2];
167 
168  v1[0] = acos(cosValue);
169  v1[1] = -v1[0];
170  v2[0] = asin(sinValue);
171  v2[1] = M_PI - v2[0];
172 
173  for(int i=0;i<2;++i) {
174  for(int j=0;j<2;++j) {
175  if(std::abs(anglereduce(v1[i]) - anglereduce(v2[j])) < tolerance) return v1[i];
176  }
177  }
178  assert(!"precondition for findFirstEqualAngle failed -> no equal angles found");
179  return 0;
180 }
181 
182 
183 }
184 
185 
186 
187 #endif
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
_T atan1(_T in1, _T in2)
int enc[10]
FloatVector FloatVector * a
FloatVector * angle
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
_T atan0(const _T in1, const _T in2)


kni
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:44