00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <stdio.h>
00023 #include <f32.h>
00024 #include <s32_to_f32.h>
00025
00026 #define NEXT(n, i) (((n) + (i)/(n)) >> 1)
00027
00028 static uint32_t u32_sqrt(uint32_t number)
00029 {
00030 uint32_t n = 1;
00031 uint32_t n1 = NEXT(n, number);
00032
00033 if (number == 0)
00034 return 0;
00035
00036 while(ABS(n1 - n) > 1) {
00037 n = n1;
00038 n1 = NEXT(n, number);
00039 }
00040 while((n1*n1) > number) {
00041 n1 -= 1;
00042 }
00043 return n1;
00044 }
00045
00046 f32 f32_sqrt(f32 f)
00047 {
00048 uint32_t a,b,c,d;
00049
00050 if (F32_IS_ZERO(f))
00051 return F32_ZERO;
00052
00053 if (F32_IS_NEG(f))
00054 return F32_NAN;
00055
00056 if(f.f32_integer) {
00057
00058 a=(uint32_t)(f.f32_integer) << 16 ;
00059 b=(uint32_t)(f.f32_decimal) << 16 ;
00060 c=u32_sqrt(a);
00061 d=u32_sqrt(0x10000 + (b/a));
00062 return f32_mul(s32_to_f32( c<<8 ), s32_to_f32( d<<8 ));
00063 }
00064 else {
00065 b=(uint32_t)(f.f32_decimal) << 16 ;
00066 return s32_to_f32(u32_sqrt(b));
00067 }
00068 }
00069