lib/kinematics/AnalyticalGuess/include/MathHelperFunctions.h
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006-2008 by Neuronics AG *
3  * support@neuronics.ch *
4  ***************************************************************************/
5 
6 #define MHF_PI 3.14159265358979323846
7 
8 #include <cmath>
9 #include <vector>
10 #include <functional>
11 #include "exception.h"
12 
13 #ifndef MATH_HELPER_FUNCTIONS
14 #define MATH_HELPER_FUNCTIONS
15 
16 
17 
18 namespace MHF {
19 
20 #ifdef WIN32
21 template<typename _T> inline double round(_T x)
22 // Copyright (C) 2001 Tor M. Aamodt, University of Toronto
23 // Permisssion to use for all purposes commercial and otherwise granted.
24 // THIS MATERIAL IS PROVIDED "AS IS" WITHOUT WARRANTY, OR ANY CONDITION OR
25 // OTHER TERM OF ANY KIND INCLUDING, WITHOUT LIMITATION, ANY WARRANTY
26 // OF MERCHANTABILITY, SATISFACTORY QUALITY, OR FITNESS FOR A PARTICULAR
27 // PURPOSE.
28 {
29  if( x > 0 ) {
30  __int64 xint = (__int64) (x+0.5);
31  if( xint % 2 ) {
32  // then we might have an even number...
33  double diff = x - (double)xint;
34  if( diff == -0.5 )
35  return double(xint-1);
36  }
37  return double(xint);
38  } else {
39  __int64 xint = (__int64) (x-0.5);
40  if( xint % 2 ) {
41  // then we might have an even number...
42  double diff = x - (double)xint;
43  if( diff == 0.5 )
44  return double(xint+1);
45  }
46  return double(xint);
47  }
48 }
49 #endif // ifdef WIN32
50 //*****************************************************************
51 
52 template<typename _T> inline short sign(_T x) { return ( (x<0) ? -1 : 1 ); }
53 
54 //*****************************************************************
55 
59 template<typename _T> struct unary_precalc_sin : public std::unary_function<_T, _T> {
60  _T operator() (_T &x) {
61  return sin(x);
62  }
63 };
64 
68 template<typename _T> struct unary_precalc_cos : public std::unary_function<_T, _T> {
69  _T operator() (_T x) {
70  return cos(x);
71  }
72 };
73 
74 
75 
76 //*****************************************************************
77 template<typename _T> inline _T atan1(_T in1, _T in2) {
78 
79  if(in1==0.0)
80  return MHF_PI+sign(in2)*MHF_PI/2;
81 
82  if(in1<0.0)
83  return atan(in2/in1)+MHF_PI;
84 
85  if( (in1>0.0) && (in2<0.0) )
86  return atan(in2/in1)+2.0*MHF_PI;
87 
88  return atan(in2/in1);
89 }
90 
91 //*****************************************************************
92 template<typename _T> inline _T acotan(const _T in) {
93  if(in == 0.0)
94  return MHF_PI/2;
95  else
96  return atan(1/in);
97 }
98 
99 //*************************************************
100 template<typename _T> inline _T atan0(const _T in1, const _T in2) {
101  if(in1 == 0.0)
102  return MHF_PI/2;
103  return atan(in2/in1);
104 }
105 
106 //*************************************************
107 template<typename _T> inline _T pow2(const _T in) {
108  return pow(in,2);
109 }
110 
111 
115 template<typename _T> inline _T rad2deg(const _T a) {
116  return a*(180.0/MHF_PI);
117 }
118 
122 template<typename _T> struct unary_rad2deg : public std::unary_function<_T, _T> {
123  _T operator() (const _T a) { return rad2deg(a); }
124 };
125 
129 template<typename _T> inline _T deg2rad(const _T a) {
130  return a*(MHF_PI/180.0);
131 }
132 
136 template<typename _T> struct unary_deg2rad : public std::unary_function<_T, _T> {
137  _T operator() (const _T a) { deg2rad(a); }
138 };
139 
140 //*************************************************
141 template<typename _T> _T inline anglereduce(const _T a) {
142  return a - floor( a/(2*MHF_PI) )*2*MHF_PI;
143 }
144 //*************************************************
145 
149 template<typename _angleT, typename _encT> inline _encT rad2enc(_angleT const& angle, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
150  // converting all parameters to _angleT (usually =double)
151  _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
152  return static_cast<_encT>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*MHF_PI) ) );
153 }
154 
158 template<typename _angleT, typename _encT> inline _angleT enc2rad(_encT const& enc, _angleT const& angleOffset, _encT const& epc, _encT const& encOffset, _encT const& rotDir) {
159  // converting all parameters to _angleT (usually = double)
160  _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
161  return _angleOffset - (_enc - _encOffset)*2.0*MHF_PI/(_epc*_rotDir);
162 }
163 
167 inline double findFirstEqualAngle(double cosValue, double sinValue, double tolerance) {
168  double v1[2], v2[2];
169 
170  v1[0] = acos(cosValue);
171  v1[1] = -v1[0];
172  v2[0] = asin(sinValue);
173  v2[1] = MHF_PI - v2[0];
174 
175  for(int i=0;i<2;++i) {
176  for(int j=0;j<2;++j) {
177  if(std::abs(anglereduce(v1[i]) - anglereduce(v2[j])) < tolerance) return v1[i];
178  }
179  }
180  throw AnaGuess::Exception("precondition for findFirstEqualAngle failed -> no equal angles found", -2);
181  return 0;
182 }
183 
184 
185 } // namespace
186 
187 
188 
189 #endif // ifndef MATH_HELPER_FUNCTIONS
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
BaseException Exception
Definition: myexcept.h:98
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
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)


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16