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 deg2rad;
184 
186 extern const double rad2deg;
187 
189 extern double epsilon;
190 
192 extern int VSIZE;
193 
194 
195 
196 #ifndef _MFC_VER
197 #undef max
198 inline double max(double a,double b) {
199  if (b<a)
200  return a;
201  else
202  return b;
203 }
204 
205 #undef min
206 inline double min(double a,double b) {
207  if (b<a)
208  return b;
209  else
210  return a;
211 }
212 #endif
213 
214 
215 #ifdef _MSC_VER
216  //#pragma inline_depth( 255 )
217  //#pragma inline_recursion( on )
218  #define INLINE __forceinline
219  //#define INLINE inline
220 #else
221  #define INLINE inline
222 #endif
223 
224 
225 inline double LinComb(double alfa,double a,
226  double beta,double b ) {
227  return alfa*a+beta*b;
228 }
229 
230 inline void LinCombR(double alfa,double a,
231  double beta,double b,double& result ) {
232  result=alfa*a+beta*b;
233  }
234 
236 inline void SetToZero(double& arg) {
237  arg=0;
238 }
239 
241 inline void SetToIdentity(double& arg) {
242  arg=1;
243 }
244 
245 inline double sign(double arg) {
246  return (arg<0)?(-1):(1);
247 }
248 
249 inline double sqr(double arg) { return arg*arg;}
250 inline double Norm(double arg) {
251  return fabs( (double)arg );
252 }
253 
254 #if defined __WIN32__ && !defined __GNUC__
255 inline double hypot(double y,double x) { return ::_hypot(y,x);}
256 inline double abs(double x) { return ::fabs(x);}
257 #endif
258 
259 // compares whether 2 doubles are equal in an eps-interval.
260 // Does not check whether a or b represents numbers
261 // On VC6, if a/b is -INF, it returns false;
262 inline bool Equal(double a,double b,double eps=epsilon)
263 {
264  double tmp=(a-b);
265  return ((eps>tmp)&& (tmp>-eps) );
266 }
267 
268 inline void random(double& a) {
269  a = 1.98*rand()/(double)RAND_MAX -0.99;
270 }
271 
272 inline void posrandom(double& a) {
273  a = 0.001+0.99*rand()/(double)RAND_MAX;
274 }
275 
276 inline double diff(double a,double b,double dt) {
277  return (b-a)/dt;
278 }
279 //inline float diff(float a,float b,double dt) {
280 //return (b-a)/dt;
281 //}
282 inline double addDelta(double a,double da,double dt) {
283  return a+da*dt;
284 }
285 
286 //inline float addDelta(float a,float da,double dt) {
287 // return a+da*dt;
288 //}
289 
290 
291 }
292 
293 
294 
295 #endif
double log(double a)
Definition: utility.h:95
const double rad2deg
the value 180/pi
double exp(double a)
Definition: utility.h:92
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
double tanh(double a)
Definition: utility.h:119
double atan2(double a, double b)
Definition: utility.h:125
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:448
int STREAMBUFFERSIZE
double sign(double arg)
Definition: utility.h:245
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:305
const double PI
the value of pi
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:345
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1123
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:385
int VSIZE
the number of derivatives used in the RN-... objects.
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:353
const T & Arg
Arg is used for passing the element to a function.
Definition: utility.h:146
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:337
int MAXLENFILENAME
maximal length of a file name
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:424
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1062
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
double cosh(double a)
Definition: utility.h:101
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:377
double pow(double a, double b)
Definition: utility.h:122
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Definition: rall1d.h:393
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
double tan(double a)
Definition: utility.h:98
double min(double a, double b)
Definition: utility.h:206
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
IMETHOD void posrandom(Vector &a)
Definition: frames.hpp:1237
double asin(double a)
Definition: utility.h:116
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:297
INLINE void SetToIdentity(Rall1d< T, V, S > &value)
Definition: rall1d.h:462
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:361
double sin(double a)
Definition: utility.h:85
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
Definition: rall1d.h:401
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:409
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:431
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.
Definition: frames.hpp:1150
double sinh(double a)
Definition: utility.h:104
double sqrt(double a)
Definition: utility.h:107
const double deg2rad
the value pi/180
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:329
double max(double a, double b)
Definition: utility.h:198
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
double cos(double a)
Definition: utility.h:89
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313
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:439
double atan(double a)
Definition: utility.h:110
double acos(double a)
Definition: utility.h:113


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36