T536_DTU/dtu/dtu_main_t536/app/floatspc.c

/*---------------------------------------------------------------------------
* 版 本 号：  1.00   版本号
* 文 件 名：  floatspc.c  
* 生成日期：  2006.04.30 
* 作    者：  钟振新
-----------------------------------------------------------------------------
单精度浮点数
   ----------------------------------
   | s  |   J     |   A(a22....a0)  |
   ----------------------------------
   | 31 |  30--23 |   22  ------  0 |
   ----------------------------------
    s:尾符，1为负数，0为正数；
    J:用移码表示的阶码，J=127 + 实际阶码；
    A:用原码表示的尾数小数部分，作为规格化的数，整数个位均为1，表示时该位隐含。
    

-----------------------------------------------------------------------------*/
#include "head.h"


/******************************************************************************
* 长整数快速求平方根，绝对误差不大于0.5，在数小于9702时会出现大于0.5%的误差，
* 在数小于2351时会出现大于1.0%的误差.
******************************************************************************/
unsigned short sqrt_16(uint32_t x)
{
    FAST unsigned short y, i;
    FAST uint32_t temp1, temp2;   /* 结果、循环计数*/
    if (x == 0)               
        return 0;
    y = 0;
    
    temp1 = (x >> 30);          /* 取最高位和次高位*/
    x <<= 2;
    if (temp1 >= 1)              
    {
        y ++;                 /* 商最高位为1*/
        temp1 -= y;
    }

    for (i = 15; i > 0; i--)      /* 求低15位，类似求最高位*/
    {
        y <<= 1;              
        temp1 <<= 2;
        temp1 += (x >> 30);     
        
        temp2 = y;
        temp2 = (temp2<<1)+1;

        x <<= 2;
        if (temp1 >= temp2)       
        {
            temp1 -= temp2;
            y ++;
        }       

    }
    if ((temp1 << 1) > temp2)   /*大于0.5末尾进一位*/
    {
        y++;
    }

    /*等于0.5且商为奇数末尾进一位(单进双不进)*/
    else if (((temp1 << 1) == temp2) && ((y & 1) == 1))
    {
        y++;
    }

    return y;
}
/*******************************************************************************
* float sqrt_32f(uint32_t x) 
* 长整数快速求平方根，返回精确浮点数,浮点数只做了一次类型转换和浮点数加法。在无浮点

* 测试：
* 1.边界测试0-0xffffffff无异常现象；
* 2.在0-0xffffffff下绝对误差(e = sqrt_32f(x) - sqrt(x))不超过±0.01，相对误差
*   e = sqrt_32f(x)/sqrt(x) - 1,不超过±0.00005。

    测试纪录：
    绝对误差超过0.01的项:无。总测试样例(0-4294967295)。
    相对误差>0.00005的值,共计94项,但不超过0.00006,总测试样例(0-4294967295)。   
*******************************************************************************/

float sqrt_32f(uint32_t x)
{
    FAST uint32_t y;
    FAST uint32_t i,m,n,temp1,temp2,*ulp ;
    float t ;

     
    if ((x == 0) || (x == 1))  return x;
    
    y = 0;
    m = 0;
    n = 0;
    ulp = (uint32_t*)&t;
    
    while(x < 16384)            /*提高小数值时的精度*/
    {
        x <<= 14;
        m++;
    }

    /*求整数根的最高位*/
    temp1 = (x >> 30);
    x <<= 2;
    if (temp1 >= 1)              
    {
        y++;                         /* 最高位为1*/
        temp1 -= y;
    }

    /*求整数根的低15位，类似求最高位*/
    for (i = 15; i > 0; i--)       
    {
        y <<= 1;              
        temp1 <<= 2;
        temp1 += (x >> 30);     
        
        temp2 = y;
        temp2 = (temp2 << 1) + 1;

        x <<= 2;
        if (temp1 >= temp2)       
        {
            temp1 -= temp2;
            y++;
        }     
    }

    /*整数根*/
    
    if ((temp1 == 0)) /*|| (temp2 == 0)) 不会出现temp2=0的情况*/
    {
        while(m)                            /*检查是否是放大了的数*/
        {
            m--;
            y >>= 7;
        }
        return y;
    }
    if (temp1 >= temp2)
    {
        y++;
        while(m)                            /*检查是否是放大了的数*/
        {
            m--;
            y >>= 7;
        }
        return y;
    }

    /*求根的小数部分，返回指定精度浮点数*/

    while (temp1 < temp2)   /*防止出现浮点数0的情况*/
    {
        n++;
        temp1 = (temp1 << 14);
    }
    
    temp1 = temp1 / temp2;      /*化小数商为整数，小数点后有效数字4位*/
    t = (float)temp1;

    while(n)
    {
        n--;
        *ulp -= 0x07000000;      /*浮点数指数减14,恢复放大的小数商*/
    }

    /*合并小数和整数*/

    t += y;


    /*检查是否是放大了根*/
    while(m)
    {
        m--;
        *ulp -= 0x03800000;      /*指数减7*/
    }

    return t;      

}
/******************************************************************************
*浮点数快速求平方根
******************************************************************************/

float sqrt_f(float x)
{
      FAST uint32_t m,y;
      FAST uint32_t i,j,k,n,temp1,temp2,*ulp;
      float t ;

      t = x;  
      j = 0;
      k = 0;
      n = 0;
      y = 0;
      
      ulp = (uint32_t*)&t;
      
      if (((*ulp) & 0x7f800000) == 0)  /* 为浮点数0 */
      {
        return 0;
      }
      
      if (((*ulp) & 0x80000000) == 0x80000000)   /*负数*/
      {
        return 0;
      }

     
      if (((*ulp) & 0x7f800000) >= 0x4f000000)  /*大于等于2^31,缩小，不会丢失有效数字*/
      {
        
        k = (((*ulp) & 0x7f800000) - 0x4f000000) & 0x7f000000 ;    /*取阶码的差，屏蔽最低位,使之成为偶数*/
       
        k += 0x01000000;      /*偶数的阶码差再加2*/
        *ulp -= k;
        k >>= 1;
      }
       
      if (((*ulp) & 0x7f800000) <= 0x47800000)  /*小于等于2^17,放大，达到较多的整数位*/
      {
        
        j = (0x47800000 - ((*ulp) & 0x7f800000))  & 0x7f000000 ;  /*取阶码的差，屏蔽最低位,使之成为偶数*/
       
        j += 0x01000000;     /*偶数的阶码差再加2*/
        *ulp += j;
        j >>= 1;
      }

      m = (uint32_t )t;      /*取调整后的浮点数的整数部分,纯小数取小数后5位小数位*/



    /*求结果的最高位*/
    temp1 = (m >> 30);
    m <<= 2;
    if (temp1 >= 1)              
    {
        y++;                         /* 最高位为1*/
        temp1 -= y;
    }

    /*求低15位，类似求最高位*/
    for (i = 15; i > 0; i--)
    {
        y <<= 1;              
        temp1 <<= 2;
        temp1 += (m >> 30);
        
        temp2 = y;
        temp2 = (temp2 << 1) + 1;

        m <<= 2;
        if (temp1 >= temp2)       
        {
            temp1 -= temp2;
            y++;
        }     
    }

    /*整数根*/
    if ((temp1 == 0)) /*|| (temp2 == 0)) 不会出现temp2=0的情况*/
    {
        t = y;
        if(j != 0)
        {
            *ulp -= j;    /* 恢复放大的根*/
        }

        if(k != 0)
        {           
           *ulp += k;     /* 恢复缩小的根*/
        }
        return t;
    }

    if (temp1 >= temp2)
    {
        y++;
        t = y;

        if(j != 0)
        {
            *ulp -= j;    /* 恢复放大的根*/
        }

        if(k != 0)
        {
         
           *ulp += k;     /* 恢复缩小的根*/
        }
             
        return t;
    }


    /*求小数位*/
    while (temp1 < temp2)           /*防止出现浮点数0的情况*/
    {
        n++;
        temp1 <<= 14;
    }

    temp1 = temp1 / temp2;         /*化小数商为整数，小数点后有效数字4位*/
    t = (float)temp1;

    while (n)
    {
        n--;
        *ulp -= 0x07000000;        /*浮点数指数减14,恢复放大的小数商*/
    }

    /*合并小数和整数*/
    t = y + t;


    /*检查是否有缩小或放大*/
    if(j != 0)
    {      
       *ulp -= j;                   /* 恢复放大的根*/
    }

    if(k != 0)
    {      
       *ulp += k;                   /* 恢复缩小的根*/
    }
        
    return t;                       /*返回浮点数根*/

}


// 浮点有效性检查
int isnan_f (void * _f)
{
    _rtl_float_t *p = (_rtl_float_t *) _f;

    if ((p->i32.s.high16.value & 0x7F80) == 0x7F80)   //阶码全为1
    {
        if (((p->i32.s.high16.value & 0x007F) == 0) && (p->i32.s.low16.value == 0))  //尾数全为0
        {
            return _FPCLASS_OVER;   //无穷大(+INF,-INF)
        }
        else   //非法数据(NaN,no a number)
        {
            if ((p->i32.s.high16.value & 0x007f) == 0)  //尾数最高位为1
            {
                return  _FPCLASS_SNAN; // Signaling NAN returns 1 发信号的NaN(S)
            }
            else   //尾数最高位为0
            {
                return  _FPCLASS_QNAN; // Quiet NAN returns 2  不发信号的NaN(Q)
            }
        }
    }

    return 0;
}


/*****************************************************************************
函数名称:DWORD sqrt_32fix(DWORD x)
函数版本:V1.00.00  
作    者:钟振新
创建日期:2007.07.25      
函数功能说明:定点数快速求平方根，高16位为整数部分，低16位为小数部分

   
输入参数: x = 无符号长整数
 
输出参数: 无
    
返 回 值: 32位定点数根
    
函数扇出清单:
函数扇入清单:
函数体中用到的公共资源:
更新后的上述资源:
******************************************************************************/
DWORD  sqrt_32fix(DWORD x)
{
    DWORD y;
    DWORD i,m,n,temp1,temp2;

	if (x == 0)
	{
		y=0;
		return y;
	}
	if (x == 1) 
	{
		y=65536;
		return y;
	}
    y = 0;
    m = 0;
    n = 0;
       
    while(x < 16384)            /*提高小数值时的精度*/
    {
        x <<= 14;
        m++;
    }
    
    /* net_section begin  */     /*net_section代码来自网络 */
    
    /*求整数根的最高位*/
    temp1 = (x >> 30);
    x <<= 2;
    if (temp1 >= 1)              
    {
        y++;                         /* 最高位为1*/
        temp1 -= y;
    }

    /*求整数根的低15位，类似求最高位*/
    for (i = 15; i > 0; i--)       
    {
        y <<= 1;
        temp1 <<= 2;
        temp1 += (x >> 30);     
        
        temp2 = y;
        temp2 = (temp2 << 1) + 1;

        x <<= 2;
        if (temp1 >= temp2)       
        {
            temp1 -= temp2;
            y++;
        }     
    }
    /* net section end *******************/

    /*整数根*/
    
    if (temp1 == 0) 
    {
        while(m)                            /*检查是否是放大了的数*/
        {
            m--;
            y >>= 7;
        }
        return (y << 16);
    }
    if (temp1 >= temp2)                     
    {
        y++;
        while(m)                            /*检查是否是放大了的数*/
        {
            m--;
            y >>= 7;
        }
        return (y << 16);
    }
    
    

    /*求根的小数部分，返回指定精度浮点数*/

    while (temp1 < temp2)   /*防止出现浮点数0的情况*/
    {
        n++;
        temp1 = (temp1 << 14);
    }

    temp1 = temp1 / temp2;      /*化小数商为整数，小数点后有效数字4位*/
	if (n == 0)
	{
		temp1 <<= 16;
	}
	while(n)
	{
		n--;
		temp1 <<= 2;            /*浮点数指数减14,恢复放大的小数商(折算为65536的倍数)*/
	}
    
    

    /*合并小数和整数*/  
    /*检查是否是放大了根*/
    while(m)
    {
        m--;
        temp1 += ((y % 128) << 16);      /*指数减7*/
        temp1 >>= 7;
        y >>= 7;                         /*指数减7*/
        
    }

    y = (y << 16) + temp1;               /*合并整数和小数部分*/

    return y;

}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*********************************************************************************************
*                                   END                                                      *
*********************************************************************************************/