Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / utilities / utility.h
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 19905 2009-04-23 13:29:54Z ben2610 $
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 accomodate 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
32 /////////////////////////////////////////////////////////////
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
49 //! turn on or off frames bounds checking. If turned on, assert() can still
50 //! be turned off with -DNDEBUG.
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.
63     using ::sin;
64     using ::cos;
65     using ::exp;
66     using ::log;
67     using ::sin;
68     using ::cos;
69     using ::tan;
70     using ::sinh;
71     using ::cosh;
72     using ::pow;
73     using ::sqrt;
74     using ::atan;
75     using ::hypot;
76     using ::asin;
77     using ::acos;
78     using ::tanh;
79     using ::atan2;
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
131
132
133
134 /** 
135  * Auxiliary class for argument types (Trait-template class )
136  * 
137  * Is used to pass doubles by value, and arbitrary objects by const reference.
138  * This is TWICE as fast (2 x less memory access) and avoids bugs in VC6++ concerning
139  * the assignment of the result of intrinsic functions to const double&-typed variables,
140  * and optimization on.
141  */
142 template <class T>
143 class TI
144 {
145     public:
146         typedef const T& Arg; //!< Arg is used for passing the element to a function.
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
165 /** 
166  * /note linkage
167  * Something fishy about the difference between C++ and C
168  * in C++ const values default to INTERNAL linkage, in C they default
169  * to EXTERNAL linkage.  Here the constants should have EXTERNAL linkage
170  * because they, for at least some of them, can be changed by the user.
171  * If you want to explicitly declare internal linkage, use "static".
172  */
173 //! 
174 extern int          STREAMBUFFERSIZE;
175
176 //! maximal length of a file name
177 extern int          MAXLENFILENAME;
178
179 //! the value of pi
180 extern const double PI;
181
182 //! the value pi/180
183 extern const double deg2rad;
184
185 //! the value 180/pi
186 extern const double rad2deg;
187
188 //! default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
189 extern double     epsilon;
190
191 //! power or 2 of epsilon
192 extern double     epsilon2;
193
194 //! the number of derivatives used in the RN-... objects.
195 extern int          VSIZE;
196
197
198
199 #ifndef _MFC_VER
200 #undef max
201 inline double max(double a,double b) {
202     if (b<a) 
203         return a;
204     else
205         return b;
206 }
207
208 #undef min
209 inline double min(double a,double b) {
210     if (b<a) 
211         return b;
212     else
213         return a;
214 }
215 #endif
216
217
218 #ifdef _MSC_VER
219     //#pragma inline_depth( 255 )
220     //#pragma inline_recursion( on )
221     #define INLINE __forceinline
222     //#define INLINE inline
223 #else
224     #define INLINE inline
225 #endif
226
227
228 inline double LinComb(double alfa,double a,
229         double beta,double b ) {
230             return alfa*a+beta*b;
231 }
232
233 inline void LinCombR(double alfa,double a,
234         double beta,double b,double& result ) {
235             result=alfa*a+beta*b;
236          }
237
238 //! to uniformly set double, RNDouble,Vector,... objects to zero in template-classes
239 inline void SetToZero(double& arg) {
240     arg=0;
241 }
242
243 //! to uniformly set double, RNDouble,Vector,... objects to the identity element in template-classes
244 inline void SetToIdentity(double& arg) {
245     arg=1;
246 }
247
248 inline double sign(double arg) {
249     return (arg<0)?(-1):(1);
250 }
251
252 inline double sqr(double arg) { return arg*arg;}
253 inline double Norm(double arg) {
254     return fabs(  (double)arg );
255 }
256
257 #ifdef __WIN32__
258 inline double hypot(double y,double x) { return ::_hypot(y,x);}
259 inline double abs(double x) { return ::fabs(x);}
260 #endif
261
262 // compares whether 2 doubles are equal in an eps-interval.
263 // Does not check whether a or b represents numbers
264 // On VC6, if a/b is -INF, it returns false;
265 inline bool Equal(double a,double b,double eps=epsilon)
266 {
267     double tmp=(a-b);
268     return ((eps>tmp)&& (tmp>-eps) );
269 }
270
271 inline void random(double& a) {
272         a = 1.98*rand()/(double)RAND_MAX -0.99;
273 }
274
275 inline void posrandom(double& a) {
276         a = 0.001+0.99*rand()/(double)RAND_MAX;
277 }
278
279 inline double diff(double a,double b,double dt) {
280         return (b-a)/dt;
281 }
282 //inline float diff(float a,float b,double dt) {
283 //return (b-a)/dt;
284 //}
285 inline double addDelta(double a,double da,double dt) {
286         return a+da*dt;
287 }
288
289 //inline float addDelta(float a,float da,double dt) {
290 //      return a+da*dt;
291 //}
292
293
294 }
295
296
297
298 #endif