orocos_kdl
utility.h
Go to the documentation of this file.
1 /*****************************************************************************
2  * \author
3  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
4  *
5  * \version
6  * ORO_Geometry V0.2
7  *
8  * \par History
9  * - $log$
10  *
11  * \par Release
12  * $Id: utility.h,v 1.1.1.1.2.4 2003/07/18 14:58:36 psoetens Exp $
13  * $Name: $
14  * \file
15  * Included by most lrl-files to provide some general
16  * functions and macro definitions.
17  *
18  * \par history
19  * - changed layout of the comments to accommodate doxygen
20  */
21 
22 
23 #ifndef KDL_UTILITY_H
24 #define KDL_UTILITY_H
25 
26 #include "kdl-config.h"
27 #include <cstdlib>
28 #include <cassert>
29 #include <cmath>
30 
31 
33 // configurable options for the frames library.
34 
35 #ifdef KDL_INLINE
36  #ifdef _MSC_VER
37  // Microsoft Visual C
38  #define IMETHOD __forceinline
39  #else
40  // Some other compiler, e.g. gcc
41  #define IMETHOD inline
42  #endif
43 #else
44  #define IMETHOD
45 #endif
46 
47 
48 
51 #ifdef KDL_INDEX_CHECK
52  #define FRAMES_CHECKI(a) assert(a)
53 #else
54  #define FRAMES_CHECKI(a)
55 #endif
56 
57 
58 namespace KDL {
59 
60 #ifdef __GNUC__
61  // so that sin,cos can be overloaded and complete
62  // resolution of overloaded functions work.
80 #endif
81 #ifndef __GNUC__
82  //only real solution : get Rall1d and varia out of namespaces.
83  #pragma warning (disable:4786)
84 
85  inline double sin(double a) {
86  return ::sin(a);
87  }
88 
89  inline double cos(double a) {
90  return ::cos(a);
91  }
92  inline double exp(double a) {
93  return ::exp(a);
94  }
95  inline double log(double a) {
96  return ::log(a);
97  }
98  inline double tan(double a) {
99  return ::tan(a);
100  }
101  inline double cosh(double a) {
102  return ::cosh(a);
103  }
104  inline double sinh(double a) {
105  return ::sinh(a);
106  }
107  inline double sqrt(double a) {
108  return ::sqrt(a);
109  }
110  inline double atan(double a) {
111  return ::atan(a);
112  }
113  inline double acos(double a) {
114  return ::acos(a);
115  }
116  inline double asin(double a) {
117  return ::asin(a);
118  }
119  inline double tanh(double a) {
120  return ::tanh(a);
121  }
122  inline double pow(double a,double b) {
123  return ::pow(a,b);
124  }
125  inline double atan2(double a,double b) {
126  return ::atan2(a,b);
127  }
128 #endif
129 
130 #if (__cplusplus > 199711L)
131 using std::isnan;
132 #endif
133 
142 template <class T>
143 class TI
144 {
145  public:
146  typedef const T& Arg;
147 };
148 
149 template <>
150 class TI<double> {
151 public:
152  typedef double Arg;
153 };
154 
155 template <>
156 class TI<int> {
157 public:
158  typedef int Arg;
159 };
160 
161 
162 
163 
164 
173 extern int STREAMBUFFERSIZE;
175 
177 extern int MAXLENFILENAME;
178 
180 extern const double PI;
181 
183 extern const double PI_2;
184 
186 extern const double PI_4;
187 
189 extern const double deg2rad;
190 
192 extern const double rad2deg;
193 
195 extern double epsilon;
196 
198 extern int VSIZE;
199 
200 
201 
202 #ifndef _MFC_VER
203 #undef max
204 inline double max(double a,double b) {
205  if (b<a)
206  return a;
207  else
208  return b;
209 }
210 
211 #undef min
212 inline double min(double a,double b) {
213  if (b<a)
214  return b;
215  else
216  return a;
217 }
218 #endif
219 
220 
221 #ifdef _MSC_VER
222  //#pragma inline_depth( 255 )
223  //#pragma inline_recursion( on )
224  #define INLINE __forceinline
225  //#define INLINE inline
226 #else
227  #define INLINE inline
228 #endif
229 
230 
231 inline double LinComb(double alfa,double a,
232  double beta,double b ) {
233  return alfa*a+beta*b;
234 }
235 
236 inline void LinCombR(double alfa,double a,
237  double beta,double b,double& result ) {
238  result=alfa*a+beta*b;
239  }
240 
242 inline void SetToZero(double& arg) {
243  arg=0;
244 }
245 
247 inline void SetToIdentity(double& arg) {
248  arg=1;
249 }
250 
251 inline double sign(double arg) {
252  return (arg<0)?(-1):(1);
253 }
254 
255 inline double sqr(double arg) { return arg*arg;}
256 inline double Norm(double arg) {
257  return fabs( (double)arg );
258 }
259 
260 #if defined __WIN32__ && !defined __GNUC__
261 inline double hypot(double y,double x) { return ::_hypot(y,x);}
262 inline double abs(double x) { return ::fabs(x);}
263 #endif
264 
265 // compares whether 2 doubles are equal in an eps-interval.
266 // Does not check whether a or b represents numbers
267 // On VC6, if a/b is -INF, it returns false;
268 inline bool Equal(double a,double b,double eps=epsilon)
269 {
270  double tmp=(a-b);
271  return ((eps>tmp)&& (tmp>-eps) );
272 }
273 
274 inline void random(double& a) {
275  a = 1.98*rand()/(double)RAND_MAX -0.99;
276 }
277 
278 inline void posrandom(double& a) {
279  a = 0.001+0.99*rand()/(double)RAND_MAX;
280 }
281 
282 inline double diff(double a,double b,double dt) {
283  return (b-a)/dt;
284 }
285 //inline float diff(float a,float b,double dt) {
286 //return (b-a)/dt;
287 //}
288 inline double addDelta(double a,double da,double dt) {
289  return a+da*dt;
290 }
291 
292 //inline float addDelta(float a,float da,double dt) {
293 // return a+da*dt;
294 //}
295 
296 
297 }
298 
299 
300 
301 #endif
KDL::SetToIdentity
INLINE void SetToIdentity(Rall1d< T, V, S > &value)
Definition: rall1d.h:484
KDL::pow
double pow(double a, double b)
Definition: utility.h:122
KDL::pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:383
KDL::posrandom
IMETHOD void posrandom(doubleVel &F)
Definition: framevel.hpp:69
KDL::random
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:65
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
KDL::sin
double sin(double a)
Definition: utility.h:85
KDL::asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Definition: rall1d.h:415
KDL::sqrt
double sqrt(double a)
Definition: utility.h:107
KDL::min
double min(double a, double b)
Definition: utility.h:212
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL::exp
double exp(double a)
Definition: utility.h:92
cmath
KDL::deg2rad
const double deg2rad
the value pi/180
Definition: utility.cxx:19
KDL::addDelta
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
KDL::exp
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:319
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::tanh
double tanh(double a)
Definition: utility.h:119
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
std::isnan
T isnan(T... args)
KDL::atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:453
KDL::asin
double asin(double a)
Definition: utility.h:116
KDL::Norm
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:440
KDL::PI_2
const double PI_2
the value of pi/2
Definition: utility.cxx:17
KDL::sinh
double sinh(double a)
Definition: utility.h:104
KDL::atan
double atan(double a)
Definition: utility.h:110
KDL::cos
double cos(double a)
Definition: utility.h:89
KDL::acos
double acos(double a)
Definition: utility.h:113
KDL::tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:351
KDL::PI
const double PI
the value of pi
Definition: utility.cxx:16
KDL::max
double max(double a, double b)
Definition: utility.h:204
KDL::TI< int >::Arg
int Arg
Definition: utility.h:158
KDL::rad2deg
const double rad2deg
the value 180/pi
Definition: utility.cxx:20
KDL::STREAMBUFFERSIZE
int STREAMBUFFERSIZE
Definition: utility.cxx:14
KDL::TI< double >::Arg
double Arg
Definition: utility.h:152
KDL::LinComb
INLINE Rall1d< T, V, S > LinComb(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b)
Definition: rall1d.h:461
KDL::sign
double sign(double arg)
Definition: utility.h:251
KDL::VSIZE
int VSIZE
the number of derivatives used in the RN-... objects.
cstdlib
KDL::log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:327
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
cassert
KDL::LinCombR
INLINE void LinCombR(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b, Rall1d< T, V, S > &result)
Definition: rall1d.h:470
KDL::sinh
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:359
KDL::atan2
double atan2(double a, double b)
Definition: utility.h:125
KDL::TI::Arg
const typedef T & Arg
Arg is used for passing the element to a function.
Definition: utility.h:146
KDL::hypot
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:407
KDL::atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:399
KDL::cosh
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:367
KDL::log
double log(double a)
Definition: utility.h:95
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::sqr
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:375
kdl-config.h
KDL::tan
double tan(double a)
Definition: utility.h:98
KDL::PI_4
const double PI_4
the value of pi/4
Definition: utility.cxx:18
KDL::cosh
double cosh(double a)
Definition: utility.h:101
KDL::tanh
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:446
KDL::TI
Definition: utility.h:143
KDL::acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
Definition: rall1d.h:423
KDL::abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:431
KDL::MAXLENFILENAME
int MAXLENFILENAME
maximal length of a file name
Definition: utility.cxx:15