IceFPU.h
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 // Include Guard
00012 #ifndef __ICEFPU_H__
00013 #define __ICEFPU_H__
00014 
00015         #define SIGN_BITMASK                    0x80000000
00016 
00018         #define IR(x)                                   ((udword&)(x))
00019 
00021         #define SIR(x)                                  ((sdword&)(x))
00022 
00024         #define AIR(x)                                  (IR(x)&0x7fffffff)
00025 
00027         #define FR(x)                                   ((float&)(x))
00028 
00031         //#define IS_NEGATIVE_FLOAT(x)  (IR(x)&0x80000000)
00032         #define IS_NEGATIVE_FLOAT(x)    ((x)<0)
00033 
00036         inline_ float FastFabs(float x)
00037         {
00038                 udword FloatBits = IR(x)&0x7fffffff;
00039                 return FR(FloatBits);
00040         }
00041 
00043         inline_ float FastSqrt(float square)
00044         {
00045 #if defined(_MSC_VER) && not defined(_WIN64)
00046                         float retval;
00047 
00048                         __asm {
00049                                         mov             eax, square
00050                                         sub             eax, 0x3F800000
00051                                         sar             eax, 1
00052                                         add             eax, 0x3F800000
00053                                         mov             [retval], eax
00054                         }
00055                         return retval;
00056 #else
00057                         return sqrt(square);
00058 #endif
00059         }
00060 
00062         inline_ float fsat(float f)
00063         {
00064                 udword y = (udword&)f & ~((sdword&)f >>31);
00065                 return (float&)y;
00066         }
00067 
00069         inline_ float frsqrt(float f)
00070         {
00071                 float x = f * 0.5f;
00072                 udword y = 0x5f3759df - ((udword&)f >> 1);
00073                 // Iteration...
00074                 (float&)y  = (float&)y * ( 1.5f - ( x * (float&)y * (float&)y ) );
00075                 // Result
00076                 return (float&)y;
00077         }
00078 
00080         inline_ float InvSqrt(const float& x)
00081         {
00082                 udword tmp = (udword(IEEE_1_0 << 1) + IEEE_1_0 - *(udword*)&x) >> 1;   
00083                 float y = *(float*)&tmp;                                             
00084                 return y * (1.47f - 0.47f * x * y * y);
00085         }
00086 
00089         inline_ float RSqrt(float number)
00090         {
00091                 long i;
00092                 float x2, y;
00093                 const float threehalfs = 1.5f;
00094 
00095                 x2 = number * 0.5f;
00096                 y  = number;
00097                 i  = * (long *) &y;
00098                 i  = 0x5f3759df - (i >> 1);
00099                 y  = * (float *) &i;
00100                 y  = y * (threehalfs - (x2 * y * y));
00101 
00102                 return y;
00103         }
00104 
00106         inline_ float fsqrt(float f)
00107         {
00108                 udword y = ( ( (sdword&)f - 0x3f800000 ) >> 1 ) + 0x3f800000;
00109                 // Iteration...?
00110                 // (float&)y = (3.0f - ((float&)y * (float&)y) / f) * (float&)y * 0.5f;
00111                 // Result
00112                 return (float&)y;
00113         }
00114 
00116         inline_ float fepsilon(float f)
00117         {
00118                 udword b = (udword&)f & 0xff800000;
00119                 udword a = b | 0x00000001;
00120                 (float&)a -= (float&)b;
00121                 // Result
00122                 return (float&)a;
00123         }
00124 
00126         inline_ bool IsNAN(float value)                         { return (IR(value)&0x7f800000) == 0x7f800000;  }
00127         inline_ bool IsIndeterminate(float value)       { return IR(value) == 0xffc00000;                               }
00128         inline_ bool IsPlusInf(float value)                     { return IR(value) == 0x7f800000;                               }
00129         inline_ bool IsMinusInf(float value)            { return IR(value) == 0xff800000;                               }
00130 
00131         inline_ bool IsValidFloat(float value)
00132         {
00133                 if(IsNAN(value))                        return false;
00134                 if(IsIndeterminate(value))      return false;
00135                 if(IsPlusInf(value))            return false;
00136                 if(IsMinusInf(value))           return false;
00137                 return true;
00138         }
00139 
00140         #define CHECK_VALID_FLOAT(x)    ASSERT(IsValidFloat(x));
00141 
00142 /*
00144         inline_ void SetFPU()
00145         {
00146                 // This function evaluates whether the floating-point
00147                 // control word is set to single precision/round to nearest/
00148                 // exceptions disabled. If these conditions don't hold, the
00149                 // function changes the control word to set them and returns
00150                 // TRUE, putting the old control word value in the passback
00151                 // location pointed to by pwOldCW.
00152                 {
00153                         uword wTemp, wSave;
00154  
00155                         __asm fstcw wSave
00156                         if (wSave & 0x300 ||            // Not single mode
00157                                 0x3f != (wSave & 0x3f) ||   // Exceptions enabled
00158                                 wSave & 0xC00)              // Not round to nearest mode
00159                         {
00160                                 __asm
00161                                 {
00162                                         mov ax, wSave
00163                                         and ax, not 300h    ;; single mode
00164                                         or  ax, 3fh         ;; disable all exceptions
00165                                         and ax, not 0xC00   ;; round to nearest mode
00166                                         mov wTemp, ax
00167                                         fldcw   wTemp
00168                                 }
00169                         }
00170                 }
00171         }
00172 */
00174         inline_ float ComputeFloatEpsilon()
00175         {
00176                 float f = 1.0f;
00177                 ((udword&)f)^=1;
00178                 return f - 1.0f;        // You can check it's the same as FLT_EPSILON
00179         }
00180 
00181         inline_ bool IsFloatZero(float x, float epsilon=1e-6f)
00182         {
00183                 return x*x < epsilon;
00184         }
00185 
00186         #define FCOMI_ST0       _asm    _emit   0xdb    _asm    _emit   0xf0
00187         #define FCOMIP_ST0      _asm    _emit   0xdf    _asm    _emit   0xf0
00188         #define FCMOVB_ST0      _asm    _emit   0xda    _asm    _emit   0xc0
00189         #define FCMOVNB_ST0     _asm    _emit   0xdb    _asm    _emit   0xc0
00190 
00191         #define FCOMI_ST1       _asm    _emit   0xdb    _asm    _emit   0xf1
00192         #define FCOMIP_ST1      _asm    _emit   0xdf    _asm    _emit   0xf1
00193         #define FCMOVB_ST1      _asm    _emit   0xda    _asm    _emit   0xc1
00194         #define FCMOVNB_ST1     _asm    _emit   0xdb    _asm    _emit   0xc1
00195 
00196         #define FCOMI_ST2       _asm    _emit   0xdb    _asm    _emit   0xf2
00197         #define FCOMIP_ST2      _asm    _emit   0xdf    _asm    _emit   0xf2
00198         #define FCMOVB_ST2      _asm    _emit   0xda    _asm    _emit   0xc2
00199         #define FCMOVNB_ST2     _asm    _emit   0xdb    _asm    _emit   0xc2
00200 
00201         #define FCOMI_ST3       _asm    _emit   0xdb    _asm    _emit   0xf3
00202         #define FCOMIP_ST3      _asm    _emit   0xdf    _asm    _emit   0xf3
00203         #define FCMOVB_ST3      _asm    _emit   0xda    _asm    _emit   0xc3
00204         #define FCMOVNB_ST3     _asm    _emit   0xdb    _asm    _emit   0xc3
00205 
00206         #define FCOMI_ST4       _asm    _emit   0xdb    _asm    _emit   0xf4
00207         #define FCOMIP_ST4      _asm    _emit   0xdf    _asm    _emit   0xf4
00208         #define FCMOVB_ST4      _asm    _emit   0xda    _asm    _emit   0xc4
00209         #define FCMOVNB_ST4     _asm    _emit   0xdb    _asm    _emit   0xc4
00210 
00211         #define FCOMI_ST5       _asm    _emit   0xdb    _asm    _emit   0xf5
00212         #define FCOMIP_ST5      _asm    _emit   0xdf    _asm    _emit   0xf5
00213         #define FCMOVB_ST5      _asm    _emit   0xda    _asm    _emit   0xc5
00214         #define FCMOVNB_ST5     _asm    _emit   0xdb    _asm    _emit   0xc5
00215 
00216         #define FCOMI_ST6       _asm    _emit   0xdb    _asm    _emit   0xf6
00217         #define FCOMIP_ST6      _asm    _emit   0xdf    _asm    _emit   0xf6
00218         #define FCMOVB_ST6      _asm    _emit   0xda    _asm    _emit   0xc6
00219         #define FCMOVNB_ST6     _asm    _emit   0xdb    _asm    _emit   0xc6
00220 
00221         #define FCOMI_ST7       _asm    _emit   0xdb    _asm    _emit   0xf7
00222         #define FCOMIP_ST7      _asm    _emit   0xdf    _asm    _emit   0xf7
00223         #define FCMOVB_ST7      _asm    _emit   0xda    _asm    _emit   0xc7
00224         #define FCMOVNB_ST7     _asm    _emit   0xdb    _asm    _emit   0xc7
00225 
00227         inline_ float FCMax2(float a, float b)
00228         {
00229 #if defined(_MSC_VER) && not defined(_WIN64)
00230                 float Res;
00231                 _asm    fld             [a]
00232                 _asm    fld             [b]
00233                 FCOMI_ST1
00234                 FCMOVB_ST1
00235                 _asm    fstp    [Res]
00236                 _asm    fcomp
00237                 return Res;
00238 #else
00239                 return (a > b) ? a : b;
00240 #endif
00241         }
00242 
00244         inline_ float FCMin2(float a, float b)
00245         {
00246 #if defined(_MSC_VER) && not defined(_WIN64)
00247                 float Res;
00248                 _asm    fld             [a]
00249                 _asm    fld             [b]
00250                 FCOMI_ST1
00251                 FCMOVNB_ST1
00252                 _asm    fstp    [Res]
00253                 _asm    fcomp
00254                 return Res;
00255 #else
00256                 return (a < b) ? a : b;
00257 #endif
00258         }
00259 
00261         inline_ float FCMax3(float a, float b, float c)
00262         {
00263 #if defined(_MSC_VER) && not defined(_WIN64)
00264                 float Res;
00265                 _asm    fld             [a]
00266                 _asm    fld             [b]
00267                 _asm    fld             [c]
00268                 FCOMI_ST1
00269                 FCMOVB_ST1
00270                 FCOMI_ST2
00271                 FCMOVB_ST2
00272                 _asm    fstp    [Res]
00273                 _asm    fcompp
00274                 return Res;
00275 #else
00276                 return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c);
00277 #endif
00278         }
00279 
00281         inline_ float FCMin3(float a, float b, float c)
00282         {
00283 #if defined(_MSC_VER) && not defined(_WIN64)
00284                 float Res;
00285                 _asm    fld             [a]
00286                 _asm    fld             [b]
00287                 _asm    fld             [c]
00288                 FCOMI_ST1
00289                 FCMOVNB_ST1
00290                 FCOMI_ST2
00291                 FCMOVNB_ST2
00292                 _asm    fstp    [Res]
00293                 _asm    fcompp
00294                 return Res;
00295 #else
00296                 return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c);
00297 #endif
00298         }
00299 
00300         inline_ int ConvertToSortable(float f)
00301         {
00302                 int& Fi = (int&)f;
00303                 int Fmask = (Fi>>31);
00304                 Fi ^= Fmask;
00305                 Fmask &= ~(1<<31);
00306                 Fi -= Fmask;
00307                 return Fi;
00308         }
00309 
00310         enum FPUMode
00311         {
00312                 FPU_FLOOR               = 0,
00313                 FPU_CEIL                = 1,
00314                 FPU_BEST                = 2,
00315 
00316                 FPU_FORCE_DWORD = 0x7fffffff
00317         };
00318 
00319         FUNCTION ICECORE_API FPUMode    GetFPUMode();
00320         FUNCTION ICECORE_API void               SaveFPU();
00321         FUNCTION ICECORE_API void               RestoreFPU();
00322         FUNCTION ICECORE_API void               SetFPUFloorMode();
00323         FUNCTION ICECORE_API void               SetFPUCeilMode();
00324         FUNCTION ICECORE_API void               SetFPUBestMode();
00325 
00326         FUNCTION ICECORE_API void               SetFPUPrecision24();
00327         FUNCTION ICECORE_API void               SetFPUPrecision53();
00328         FUNCTION ICECORE_API void               SetFPUPrecision64();
00329         FUNCTION ICECORE_API void               SetFPURoundingChop();
00330         FUNCTION ICECORE_API void               SetFPURoundingUp();
00331         FUNCTION ICECORE_API void               SetFPURoundingDown();
00332         FUNCTION ICECORE_API void               SetFPURoundingNear();
00333 
00334         FUNCTION ICECORE_API int                intChop(const float& f);
00335         FUNCTION ICECORE_API int                intFloor(const float& f);
00336         FUNCTION ICECORE_API int                intCeil(const float& f);
00337 
00338 #endif // __ICEFPU_H__


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54