| Line 19... |
Line 19... |
* atan(x)=b+atan((x-atan(b))/(1+x*atan(b)))
|
* atan(x)=b+atan((x-atan(b))/(1+x*atan(b)))
|
* ln(a*x)=ln(a)+ln(x); ln(x^n)=n*ln(x)
|
* ln(a*x)=ln(a)+ln(x); ln(x^n)=n*ln(x)
|
*--------------------------------------------------------------------*/
|
*--------------------------------------------------------------------*/
|
#include "rtos.h"
|
#include "rtos.h"
|
|
|
|
//#define USE_SW_MULT
|
|
#if !defined(WIN32) && !defined(USE_SW_MULT)
|
|
#define USE_MULT64
|
|
#endif
|
|
|
#define PI ((float)3.1415926)
|
#define PI ((float)3.1415926)
|
#define PI_2 ((float)(PI/2.0))
|
#define PI_2 ((float)(PI/2.0))
|
#define PI2 ((float)(PI*2.0))
|
#define PI2 ((float)(PI*2.0))
|
|
|
#define FtoL(X) (*(unsigned long*)&(X))
|
#define FtoL(X) (*(unsigned long*)&(X))
|
| Line 98... |
Line 103... |
float FP_Mult(float a_fp, float b_fp)
|
float FP_Mult(float a_fp, float b_fp)
|
{
|
{
|
unsigned long a, b, c;
|
unsigned long a, b, c;
|
unsigned long as, af, bs, bf, cs, cf;
|
unsigned long as, af, bs, bf, cs, cf;
|
long ae, be, ce;
|
long ae, be, ce;
|
#ifdef WIN32
|
#ifndef USE_MULT64
|
unsigned long a2, a1, b2, b1, med1, med2;
|
unsigned long a2, a1, b2, b1, med1, med2;
|
#endif
|
#endif
|
unsigned long hi, lo;
|
unsigned long hi, lo;
|
a = FtoL(a_fp);
|
a = FtoL(a_fp);
|
b = FtoL(b_fp);
|
b = FtoL(b_fp);
|
| Line 111... |
Line 116... |
af = 0x00800000 | (a & 0x007fffff);
|
af = 0x00800000 | (a & 0x007fffff);
|
bs = b >> 31;
|
bs = b >> 31;
|
be = (b >> 23) & 0xff;
|
be = (b >> 23) & 0xff;
|
bf = 0x00800000 | (b & 0x007fffff);
|
bf = 0x00800000 | (b & 0x007fffff);
|
cs = as ^ bs;
|
cs = as ^ bs;
|
#ifdef WIN32
|
#ifndef USE_MULT64
|
a1 = af & 0xffff;
|
a1 = af & 0xffff;
|
a2 = af >> 16;
|
a2 = af >> 16;
|
b1 = bf & 0xffff;
|
b1 = bf & 0xffff;
|
b2 = bf >> 16;
|
b2 = bf >> 16;
|
lo = a1 * b1;
|
lo = a1 * b1;
|
| Line 147... |
Line 152... |
float FP_Div(float a_fp, float b_fp)
|
float FP_Div(float a_fp, float b_fp)
|
{
|
{
|
unsigned long a, b, c;
|
unsigned long a, b, c;
|
unsigned long as, af, bs, bf, cs, cf;
|
unsigned long as, af, bs, bf, cs, cf;
|
unsigned long a1, b1;
|
unsigned long a1, b1;
|
#if WIN32
|
#ifndef USE_MULT64
|
unsigned long a2, b2, med1, med2;
|
unsigned long a2, b2, med1, med2;
|
#endif
|
#endif
|
unsigned long hi, lo;
|
unsigned long hi, lo;
|
long ae, be, ce, d;
|
long ae, be, ce, d;
|
a = FtoL(a_fp);
|
a = FtoL(a_fp);
|
| Line 167... |
Line 172... |
a1 = af << 4; //8
|
a1 = af << 4; //8
|
b1 = bf >> 8;
|
b1 = bf >> 8;
|
cf = a1 / b1;
|
cf = a1 / b1;
|
cf <<= 12; //8
|
cf <<= 12; //8
|
#if 1 /*non-quick*/
|
#if 1 /*non-quick*/
|
#if WIN32
|
#ifndef USE_MULT64
|
a1 = cf & 0xffff;
|
a1 = cf & 0xffff;
|
a2 = cf >> 16;
|
a2 = cf >> 16;
|
b1 = bf & 0xffff;
|
b1 = bf & 0xffff;
|
b2 = bf >> 16;
|
b2 = bf >> 16;
|
lo = a1 * b1;
|
lo = a1 * b1;
|
| Line 510... |
Line 515... |
{
|
{
|
return FP_Exp(y * FP_Log(x));
|
return FP_Exp(y * FP_Log(x));
|
}
|
}
|
|
|
|
|
|
/********************************************/
|
|
//These five functions will only be used if the flag "-mno-mul" is enabled
|
|
#ifdef USE_SW_MULT
|
|
unsigned long __mulsi3(unsigned long a, unsigned long b)
|
|
{
|
|
unsigned long answer = 0;
|
|
int i;
|
|
for(i = 0; i < 32; ++i)
|
|
{
|
|
if(b & 1)
|
|
answer += a;
|
|
a <<= 1;
|
|
b >>= 1;
|
|
}
|
|
return answer;
|
|
}
|
|
|
|
|
|
static unsigned long DivideMod(unsigned long a, unsigned long b, int doMod)
|
|
{
|
|
unsigned long upper=a, lower=0;
|
|
int i;
|
|
a = b << 31;
|
|
for(i = 0; i < 32; ++i)
|
|
{
|
|
lower = lower << 1;
|
|
if(upper >= a && a && b < 2)
|
|
{
|
|
upper = upper - a;
|
|
lower |= 1;
|
|
}
|
|
a = ((b&2) << 30) | (a >> 1);
|
|
b = b >> 1;
|
|
}
|
|
if(!doMod)
|
|
return lower;
|
|
return upper;
|
|
}
|
|
|
|
|
|
unsigned long __udivsi3(unsigned long a, unsigned long b)
|
|
{
|
|
return DivideMod(a, b, 0);
|
|
}
|
|
|
|
|
|
long __divsi3(long a, long b)
|
|
{
|
|
long answer, negate=0;
|
|
if(a < 0)
|
|
{
|
|
a = -a;
|
|
negate = !negate;
|
|
}
|
|
if(b < 0)
|
|
{
|
|
b = -b;
|
|
negate = !negate;
|
|
}
|
|
answer = DivideMod(a, b, 0);
|
|
if(negate)
|
|
answer = -answer;
|
|
return answer;
|
|
}
|
|
|
|
|
|
unsigned long __umodsi3(unsigned long a, unsigned long b)
|
|
{
|
|
return DivideMod(a, b, 1);
|
|
}
|
|
#endif
|
|
|
|
|
/*************** Test *****************/
|
/*************** Test *****************/
|
#ifdef WIN32
|
#ifdef WIN32
|
#include <math.h>
|
#include <math.h>
|
#undef printf
|
#undef printf
|
int printf(const char *, ...);
|
int printf(const char *, ...);
|
