Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00034
00035 #ifdef KDL_INLINE
00036 #ifdef _MSC_VER
00037
00038 #define IMETHOD __forceinline
00039 #else
00040
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
00062
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
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
00217
00218 #define INLINE __forceinline
00219
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
00260
00261
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
00280
00281
00282 inline double addDelta(double a,double da,double dt) {
00283 return a+da*dt;
00284 }
00285
00286
00287
00288
00289
00290
00291 }
00292
00293
00294
00295 #endif