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
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 #ifndef MATHSUP_INCLUDEDEFX_H
00055 #define MATHSUP_INCLUDEDEFX_H
00056
00057 #include <math.h>
00058
00059
00060
00065 class MathSup
00066 {
00067 public:
00068
00070 static const double PI;
00071
00073 static const double TWO_PI;
00074
00076 static const double HALF_PI;
00077
00078
00082 static double convRadToDeg(const double& dAngRad)
00083 {
00084 return (dAngRad * 180.0 / PI);
00085 }
00086
00090 static double convDegToRad(const double& dAngDeg)
00091 {
00092 return ( dAngDeg * PI / 180.0 );
00093 }
00094
00098 static void normalize2Pi(double& angle)
00099 {
00100 angle-=floor(angle/(2 * PI))* 2 * PI;
00101 }
00102
00106 static void normalizePi(double& angle)
00107 {
00108 normalize2Pi(angle);
00109 if ( angle> PI ) angle-=2 * PI;
00110 }
00111
00115 static void normalizePiHalf(double& angle)
00116 {
00117 normalize2Pi(angle);
00118 if (angle > PI) angle-=2*PI;
00119 if (angle > PI/2.0) angle -= PI;
00120 if (angle <= -PI/2.0) angle += PI;
00121 }
00122
00126 static double sign(const double& x)
00127 {
00128 if (x<0)
00129 return -1.0;
00130 else
00131 return 1.0;
00132 }
00133
00137 static double getMin(const double& a, const double& b)
00138 {
00139 if (a < b)
00140 return a;
00141 else
00142 return b;
00143 }
00144
00148 static double getMax(const double& a, const double& b)
00149 {
00150 if (a > b)
00151 return a;
00152 else
00153 return b;
00154 }
00155
00160 static double calcDeltaAng(const double& a, const double& b)
00161 {
00162 double c = a-b;
00163 normalizePi(c);
00164 return c;
00165 }
00166
00167
00171 static double atan4quad(double y, double x)
00172 {
00173 double result;
00174
00175 if((x==0.0) && (y==0.0))
00176 result = 0.0;
00177 else if((x==0.0) && (y > 0.0))
00178 result = HALF_PI;
00179 else if((x==0.0) && (y < 0.0))
00180 result = -HALF_PI;
00181 else if((y==0.0) && (x > 0.0))
00182 result = 0.0;
00183 else if((y==0.0) && (x < 0.0))
00184 result = PI;
00185 else
00186 {
00187 result = atan(y/x);
00188 if(x<0.0)
00189 {
00190 if(y>0.0)
00191 result += PI;
00192 else
00193 result -= PI;
00194 }
00195 }
00196 normalizePi(result);
00197 return result;
00198 }
00199
00200
00204 static double distance(double x1, double y1, double x2, double y2)
00205 {
00206 return sqrt( distanceSq( x1, y1, x2, y2 ) );
00207 }
00208
00212 static double distanceSq(double x1, double y1, double x2, double y2)
00213 {
00214 double dx = x2 - x1;
00215 double dy = y2 - y1;
00216
00217 return dx*dx + dy*dy;
00218 }
00219
00223 static bool isBitSet(int iVal, int iNrBit)
00224 {
00225 if( (iVal & (1 << iNrBit)) == 0)
00226 return false;
00227 else
00228 return true;
00229 }
00230
00235 static double convFloatToInt4Byte(double dVal)
00236 {
00237
00238 return 1.0;
00239 }
00240
00245 static double convInt4ByteToFloat(int iVal)
00246 {
00247 unsigned char c[4];
00248 double dSign;
00249 double dFraction;
00250 double dExp;
00251 double dVal;
00252
00253 c[0] = iVal >> 24;
00254 c[1] = iVal >> 16;
00255 c[2] = iVal >> 8;
00256 c[3] = iVal;
00257
00258 if( (c[0] & 0x80) == 0)
00259 dSign = 1;
00260 else
00261 dSign = -1;
00262
00263 dFraction = (iVal & 0xFFFFFF) | 0x800000;
00264 dFraction = dFraction * pow(2., -23);
00265
00266 dExp = ((c[0] << 1) | (c[1] >> 7)) - 127;
00267
00268 dVal = dSign * dFraction * pow(10., dExp);
00269
00270 return dVal;
00271 }
00272
00280 static int limit(double* pdToLimit, double dLimit)
00281 {
00282 int iRet = 0;
00283
00284 if(*pdToLimit < -dLimit)
00285 {
00286 *pdToLimit = -dLimit;
00287 iRet = 1;
00288 }
00289 if(*pdToLimit > dLimit)
00290 {
00291 *pdToLimit = dLimit;
00292 iRet = 2;
00293 }
00294
00295 return iRet;
00296 }
00297
00305 static int limit(int* piToLimit, int iLimit)
00306 {
00307 int iRet = 0;
00308
00309 if(*piToLimit < -iLimit)
00310 {
00311 *piToLimit = -iLimit;
00312 iRet = 1;
00313 }
00314 if(*piToLimit > iLimit)
00315 {
00316 *piToLimit = iLimit;
00317 iRet = 2;
00318 }
00319
00320 return iRet;
00321 }
00322
00330 static bool isInInterval(double dLow, double dHigh, double dVal)
00331 {
00332 if( (dVal >= dLow) && (dVal <= dHigh) )
00333 return true;
00334
00335 else
00336 return false;
00337 }
00338
00339 };
00340
00341
00342
00343 #endif