matlab_23550/Inu/Src2/main/IQmathLib.c

#include "IQmathLib.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 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;
}
//
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;
}