utility.h
Go to the documentation of this file.
00001 /***************************************************************************** 
00002  *  \author 
00003  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00004  *
00005  *  \version 
00006  *              ORO_Geometry V0.2
00007  *
00008  *      \par History
00009  *              - $log$
00010  *
00011  *      \par Release
00012  *              $Id: utility.h,v 1.1.1.1.2.4 2003/07/18 14:58:36 psoetens Exp $
00013  *              $Name:  $ 
00014  *  \file
00015  *    Included by most lrl-files to provide some general
00016  *    functions and macro definitions.
00017  *  
00018  *  \par history
00019  *   - changed layout of the comments to accomodate doxygen
00020  */
00021 
00022 
00023 #ifndef KDL_UTILITY_H
00024 #define KDL_UTILITY_H
00025 
00026 #include "kdl-config.h"
00027 #include <cstdlib>
00028 #include <cassert>
00029 #include <cmath>
00030 
00031 
00033 // configurable options for the frames library.
00034 
00035 #ifdef KDL_INLINE
00036     #ifdef _MSC_VER
00037         // Microsoft Visual C
00038         #define IMETHOD __forceinline
00039     #else
00040         // Some other compiler, e.g. gcc
00041         #define IMETHOD inline
00042     #endif
00043 #else
00044     #define IMETHOD
00045 #endif
00046 
00047 
00048 
00051 #ifdef KDL_INDEX_CHECK
00052     #define FRAMES_CHECKI(a) assert(a)
00053 #else
00054     #define FRAMES_CHECKI(a)
00055 #endif
00056 
00057 
00058 namespace KDL {
00059 
00060 #ifdef __GNUC__
00061     // so that sin,cos can be overloaded and complete 
00062     // resolution of overloaded functions work.
00063     using ::sin;
00064     using ::cos;
00065     using ::exp;
00066     using ::log;
00067     using ::sin;
00068     using ::cos;
00069     using ::tan;
00070     using ::sinh;
00071     using ::cosh;
00072     using ::pow;
00073     using ::sqrt;
00074     using ::atan;
00075     using ::hypot;
00076     using ::asin;
00077     using ::acos;
00078     using ::tanh;
00079     using ::atan2;
00080 #endif
00081 #ifndef __GNUC__
00082     //only real solution : get Rall1d and varia out of namespaces.
00083     #pragma warning (disable:4786)
00084 
00085     inline double sin(double a) {
00086         return ::sin(a);
00087     }
00088     
00089     inline double cos(double a) {
00090         return ::cos(a);
00091     }
00092     inline double exp(double a) {
00093         return ::exp(a);
00094     }
00095     inline double log(double a) {
00096         return ::log(a);
00097     }
00098     inline double tan(double a) {
00099         return ::tan(a);
00100     }
00101     inline double cosh(double a) {
00102         return ::cosh(a);
00103     }
00104     inline double sinh(double a) {
00105         return ::sinh(a);
00106     }
00107     inline double sqrt(double a) {
00108         return ::sqrt(a);
00109     }
00110     inline double atan(double a) {
00111         return ::atan(a);
00112     }
00113     inline double acos(double a) {
00114         return ::acos(a);
00115     }
00116     inline double asin(double a) {
00117         return ::asin(a);
00118     }
00119     inline double tanh(double a) {
00120         return ::tanh(a);
00121     }
00122     inline double pow(double a,double b) {
00123         return ::pow(a,b);
00124     }
00125     inline double atan2(double a,double b) {
00126         return ::atan2(a,b);
00127     }
00128 #endif  
00129 
00130 
00131 
00132 
00133 
00142 template <class T>
00143 class TI
00144 {
00145     public:
00146         typedef const T& Arg; 
00147 };
00148 
00149 template <>
00150 class TI<double> {
00151 public:
00152     typedef double Arg;
00153 };
00154 
00155 template <>
00156 class TI<int> {
00157 public:
00158     typedef int Arg;
00159 };
00160 
00161 
00162 
00163 
00164 
00173 
00174 extern int          STREAMBUFFERSIZE;
00175 
00177 extern int          MAXLENFILENAME;
00178 
00180 extern const double PI;
00181 
00183 extern const double deg2rad;
00184 
00186 extern const double rad2deg;
00187 
00189 extern double     epsilon;
00190 
00192 extern int          VSIZE;
00193 
00194 
00195 
00196 #ifndef _MFC_VER
00197 #undef max
00198 inline double max(double a,double b) {
00199     if (b<a) 
00200         return a;
00201     else
00202         return b;
00203 }
00204 
00205 #undef min
00206 inline double min(double a,double b) {
00207     if (b<a) 
00208         return b;
00209     else
00210         return a;
00211 }
00212 #endif
00213 
00214 
00215 #ifdef _MSC_VER
00216     //#pragma inline_depth( 255 )
00217     //#pragma inline_recursion( on )
00218     #define INLINE __forceinline
00219     //#define INLINE inline
00220 #else
00221     #define INLINE inline
00222 #endif
00223 
00224 
00225 inline double LinComb(double alfa,double a,
00226         double beta,double b ) {
00227             return alfa*a+beta*b;
00228 }
00229 
00230 inline void LinCombR(double alfa,double a,
00231         double beta,double b,double& result ) {
00232             result=alfa*a+beta*b;
00233          }
00234 
00236 inline void SetToZero(double& arg) {
00237     arg=0;
00238 }
00239 
00241 inline void SetToIdentity(double& arg) {
00242     arg=1;
00243 }
00244 
00245 inline double sign(double arg) {
00246     return (arg<0)?(-1):(1);
00247 }
00248 
00249 inline double sqr(double arg) { return arg*arg;}
00250 inline double Norm(double arg) {
00251     return fabs(  (double)arg );
00252 }
00253 
00254 #if defined __WIN32__ && !defined __GNUC__
00255 inline double hypot(double y,double x) { return ::_hypot(y,x);}
00256 inline double abs(double x) { return ::fabs(x);}
00257 #endif
00258 
00259 // compares whether 2 doubles are equal in an eps-interval.
00260 // Does not check whether a or b represents numbers
00261 // On VC6, if a/b is -INF, it returns false;
00262 inline bool Equal(double a,double b,double eps=epsilon)
00263 {
00264     double tmp=(a-b);
00265     return ((eps>tmp)&& (tmp>-eps) );
00266 }
00267 
00268 inline void random(double& a) {
00269         a = 1.98*rand()/(double)RAND_MAX -0.99;
00270 }
00271 
00272 inline void posrandom(double& a) {
00273         a = 0.001+0.99*rand()/(double)RAND_MAX;
00274 }
00275 
00276 inline double diff(double a,double b,double dt) {
00277         return (b-a)/dt;
00278 }
00279 //inline float diff(float a,float b,double dt) {
00280 //return (b-a)/dt;
00281 //}
00282 inline double addDelta(double a,double da,double dt) {
00283         return a+da*dt;
00284 }
00285 
00286 //inline float addDelta(float a,float da,double dt) {
00287 //      return a+da*dt;
00288 //}
00289 
00290 
00291 }
00292 
00293 
00294 
00295 #endif


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14