229 lines
9.2 KiB
C
229 lines
9.2 KiB
C
#include "IQmathLib.h"
|
||
#include <math.h>
|
||
|
||
|
||
// Преобразование числа с плавающей точкой в число с фиксированной точкой
|
||
#define float_to_fixed(A) (long)((A)*(1 << (GLOBAL_Q)) + (A > 0 ? 0.5: -0.5))
|
||
// Преобразование числа с плавающей точкой в число с фиксированной точкой с выбором числа бит, отдаваемых под дробную часть
|
||
#define float_to_fixed_base_select(A, F_BITS) (long)((A)*(1 << (F_BITS)) + (A > 0 ? 0.5: -0.5))
|
||
// Преобразование целого числа в число с фиксированной точкой
|
||
#define int_to_fixed(A) (long)((A) << (GLOBAL_Q))
|
||
// Преобразование целого числа в число с фиксированной точкой с выбором числа бит, отдаваемых под дробную часть
|
||
#define int_to_fixed_base_select(A, F_BITS) (long)((A) << (F_BITS))
|
||
//Преобразование числа с фиксированной точкой в число с плавающей точкой
|
||
#define fixed_to_float(A) ((double)A / (1 << GLOBAL_Q))
|
||
//Перобразование числа с фиксированной точкой в целое число
|
||
#define fixed_to_int(A) ((int)(A >> GLOBAL_Q) )
|
||
|
||
long _IQmag(long a, long b)
|
||
{
|
||
return _IQsqrt(_IQmpy(a, a) + _IQmpy(b, b));
|
||
}
|
||
|
||
long multiply(long x, long y)
|
||
{
|
||
long long z = (long long)x * (long long)y;
|
||
return (long)(z >> GLOBAL_Q);
|
||
}
|
||
//служебная функция. Умножает числа с 27 битами, отданными под дробную часть
|
||
static inline long multiply_27(long x, long y)
|
||
{
|
||
long long z = (long long)x * (long long)y;
|
||
return z & 0x4000000 ? (long)(z >> 27) + 1 : (long)(z >> 27);
|
||
}
|
||
|
||
long long multiply_fixed_base_select(long long x, long long y, int base)
|
||
{
|
||
long long z = (long long)x * (long long)y;
|
||
return z & (1 << base) ? (z >> base) + 1 : (z >> base);
|
||
}
|
||
|
||
long divide(long num, long den)
|
||
{
|
||
long long numLong = (long long)num;
|
||
long long quotient = (numLong << GLOBAL_Q) / den;
|
||
return (long)quotient;
|
||
}
|
||
|
||
long divide19(long num, long den)
|
||
{
|
||
long long numLong = (long long)num;
|
||
long long quotient = (numLong << 19) / den;
|
||
return (long)quotient;
|
||
}
|
||
|
||
long divideN(long num, long den, unsigned int d)
|
||
{
|
||
long long numLong = (long long)num;
|
||
long long quotient = (numLong << d) / den;
|
||
return (long)quotient;
|
||
}
|
||
//
|
||
static inline long long divide_fixed_base_select(long long num, long long den, int base)
|
||
{
|
||
long long quotient = ((long long)num << base) / den;
|
||
return quotient;
|
||
}
|
||
|
||
#define div_def(A,B) (long)(((long long)(A) << 24)/(B))
|
||
#define div_mod(A,B) (A)%(B)
|
||
#define mult_def(A,B) (long)((((long long)(A))*((long long)(B))) >> 24)
|
||
#define abs_def(A) ((A) > 0 ? (A): -(A))
|
||
|
||
long sin_fixed(long x)
|
||
{
|
||
//Константы сделал ститическими, что бы они вычислялись во время запуска программы, а не исполнения
|
||
static long FIXED_2PI = float_to_fixed(TWO_PI);
|
||
static long FIXED_PI = float_to_fixed(PI);
|
||
static long FIXED_PIna2 = float_to_fixed(PI_2);
|
||
//Здесть так же что бы не производить операции деления посчитал констаны ряда Тейлора
|
||
static long one_110 = float_to_fixed_base_select(1./110, 27);
|
||
static long one_72 = float_to_fixed_base_select(1./72, 27);
|
||
static long one_42 = float_to_fixed_base_select(1./42, 27);
|
||
static long one_20= float_to_fixed_base_select(1./20, 27);
|
||
static long one_6 = float_to_fixed_base_select(1./6, 27);
|
||
|
||
long long xx, tmp ;
|
||
while(x >= FIXED_2PI) { x -= FIXED_2PI;} //Помещаю аргумент в диапазон 2 ПИ
|
||
while(x <= -FIXED_2PI) { x += FIXED_2PI;}
|
||
//Так как ряды быстрее сходнятся при малых значениях, помещаю значение аргумента
|
||
//в ближайшие к нулю области
|
||
if(x > FIXED_PI)
|
||
{
|
||
x -= FIXED_2PI;
|
||
}
|
||
else if(x < -FIXED_PI)
|
||
{
|
||
x += FIXED_2PI;
|
||
}
|
||
if(x < -FIXED_PIna2)
|
||
{
|
||
x = -FIXED_PI - x;
|
||
}
|
||
else if(x > FIXED_PIna2)
|
||
{
|
||
x = FIXED_PI - x;
|
||
}
|
||
//проверяю угол на значения, при которых синус раве 0 или 1
|
||
if(x == 0) return 0;
|
||
if(x == FIXED_PIna2) return int_to_fixed(1);
|
||
if(x == -FIXED_PIna2) return int_to_fixed(-1);
|
||
//Перевожу в формат с максимальной точностью для возможного дипазано значений
|
||
x <<= (27 - GLOBAL_Q);
|
||
//Считаю ряд фурье
|
||
xx = multiply_27(x, x);
|
||
tmp = ONE_27 - multiply_27(one_110, xx);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_72);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_42);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_20);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_6);
|
||
tmp = multiply_27(x, tmp);
|
||
return tmp >> (27 - GLOBAL_Q); //Перед возвращением из функции преобразую в первоначальный формат
|
||
}
|
||
|
||
long cos_fixed(long x)
|
||
{
|
||
//Константы сделал ститическими, что бы они вычислялись во время запуска программы, а не исполнения
|
||
static long FIXED_2PI = float_to_fixed(TWO_PI);
|
||
static long FIXED_PI = float_to_fixed(PI);
|
||
static long FIXED_PIna2 = float_to_fixed(PI_2);
|
||
//Здесть так же что бы не производить операции деления посчитал констаны ряда Тейлора
|
||
static long one_132 = float_to_fixed_base_select(1./132, 27);
|
||
static long one_90 = float_to_fixed_base_select(1./90, 27);
|
||
static long one_56 = float_to_fixed_base_select(1./56, 27);
|
||
static long one_30 = float_to_fixed_base_select(1./30, 27);
|
||
static long one_12 = float_to_fixed_base_select(1./12, 27);
|
||
|
||
long xx, tmp, counter = 0;
|
||
while(x >= FIXED_2PI) { x -= FIXED_2PI;} //Помещаю аргумент в диапазон 2 ПИ
|
||
while(x < 0) { x += FIXED_2PI;}
|
||
x = _IQabs(x); //Так как косинус симметричен относительно нуля, нахожу его модуль
|
||
//проверяю угол на значения, при которых синус раве 0 или 1
|
||
if(x == 0) return 1 << GLOBAL_Q;
|
||
if(x == FIXED_PI) return -(1 << GLOBAL_Q);
|
||
if(x == (FIXED_PIna2) || (x == FIXED_3PIna2))return 0;
|
||
//Так как ряды быстрее сходнятся при малых значениях, помещаю значение аргумента
|
||
//в ближайшие к нулю области
|
||
while(x > FIXED_PIna2)
|
||
{
|
||
x -= FIXED_PIna2;
|
||
counter++;
|
||
}
|
||
|
||
if(counter == 1 || counter == 3) { x = FIXED_PIna2 - x;}
|
||
//Перевожу в формат с максимальной точностью для возможного дипазона значений
|
||
x <<= (27 - GLOBAL_Q);
|
||
//Считаю ряд фурье
|
||
xx = multiply_27(x, x);
|
||
tmp = ONE_27 - multiply_27(xx, one_132);
|
||
tmp= multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(xx, one_90);
|
||
tmp= multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_56);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_30);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - multiply_27(tmp, one_12);
|
||
tmp = multiply_27(xx, tmp);
|
||
tmp = ONE_27 - (tmp >> 1);
|
||
tmp >>= (27 - GLOBAL_Q);
|
||
return (counter == 0) || (counter == 3) ? tmp : -tmp;
|
||
}
|
||
|
||
long sqrt_fixed(long x)
|
||
{
|
||
int variable_size_bits = sizeof(x) << 3;
|
||
long average_val, prev_avg_val;
|
||
if(x <= 0) return 0;
|
||
while(!(x & (1 << --variable_size_bits))); //Нахожу старший значащий бит
|
||
//Нахожу приближение корня сдвгом на половину числа бит между старшим значащим битом
|
||
//и положением точки
|
||
if(variable_size_bits > GLOBAL_Q)
|
||
{
|
||
average_val = x >> ((variable_size_bits - GLOBAL_Q) >> 1);
|
||
}
|
||
else
|
||
{
|
||
average_val = x << ((GLOBAL_Q - variable_size_bits) >> 1);
|
||
}
|
||
prev_avg_val = divide(x, average_val); //Нахожу 1/А
|
||
//В цикле нахожу среднее арифметическое между А и 1/А, пока число не перестанет меняться
|
||
while(_IQabs(prev_avg_val - average_val) > 1)
|
||
{
|
||
prev_avg_val = average_val;
|
||
average_val = (average_val + divide(x, average_val)) >> 1;
|
||
}
|
||
return average_val;
|
||
}
|
||
|
||
|
||
long exp_fixed(long x)
|
||
{
|
||
// static long FIXED_2PI = float_to_fixed(TWO_PI);
|
||
float f = _IQtoF(x);
|
||
float r1 = exp(f);
|
||
if (r1>127) r1=127;
|
||
if (r1<-127) r1=-127;
|
||
long r2 = _IQ(r1);
|
||
|
||
return r2;
|
||
}
|
||
|
||
|
||
long exp_fixedN(long x, unsigned int n)
|
||
{
|
||
if (n==18)
|
||
{
|
||
float f = _IQ18toF(x);
|
||
float r1 = exp(f);
|
||
if (r1>8100) r1=8100;
|
||
if (r1<-8100) r1=-8100;
|
||
long r2 = _IQ(r1);
|
||
|
||
return r2;
|
||
}
|
||
} |