157 lines
3.2 KiB
C
157 lines
3.2 KiB
C
#include "fixedpoint.h"
|
|
|
|
#include <limits.h>
|
|
#include <stddef.h>
|
|
|
|
static const int32_t s_fixed_atan_table_q30[] = {
|
|
843314857, 497837829, 263043836, 133525159,
|
|
67021687, 33543516, 16775851, 8388437,
|
|
4194283, 2097149, 1048575, 524288,
|
|
262144, 131072, 65536, 32768,
|
|
};
|
|
|
|
static uint32_t fixed_isqrt_u64(uint64_t value)
|
|
{
|
|
uint64_t bit = (uint64_t)1 << 62;
|
|
uint64_t result = 0;
|
|
|
|
while (bit > value)
|
|
{
|
|
bit >>= 2;
|
|
}
|
|
|
|
while (bit != 0)
|
|
{
|
|
if (value >= result + bit)
|
|
{
|
|
value -= result + bit;
|
|
result = (result >> 1) + bit;
|
|
}
|
|
else
|
|
{
|
|
result >>= 1;
|
|
}
|
|
|
|
bit >>= 2;
|
|
}
|
|
|
|
return (uint32_t)result;
|
|
}
|
|
|
|
int32_t fixed_sqrt(int32_t value, uint8_t frac_bits)
|
|
{
|
|
uint64_t scaled_value;
|
|
uint32_t root;
|
|
|
|
if (value <= 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
scaled_value = ((uint64_t)(uint32_t)value) << frac_bits;
|
|
root = fixed_isqrt_u64(scaled_value);
|
|
if (root > (uint32_t)INT32_MAX)
|
|
{
|
|
return INT32_MAX;
|
|
}
|
|
|
|
return (int32_t)root;
|
|
}
|
|
|
|
int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits)
|
|
{
|
|
int32_t root;
|
|
|
|
if (value <= 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
root = fixed_sqrt(value, frac_bits);
|
|
return FIXED_DIV(FIXED_ONE(frac_bits), root, frac_bits);
|
|
}
|
|
|
|
int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits)
|
|
{
|
|
int64_t x_q30;
|
|
int64_t y_q30;
|
|
int64_t angle_q30 = 0;
|
|
int shift;
|
|
|
|
if (x == 0)
|
|
{
|
|
if (y > 0)
|
|
{
|
|
return FIXED_HALF_PI(frac_bits);
|
|
}
|
|
if (y < 0)
|
|
{
|
|
return -FIXED_HALF_PI(frac_bits);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
if (x < 0)
|
|
{
|
|
if (y >= 0)
|
|
{
|
|
return fixed_atan2(-y, -x, frac_bits) + FIXED_PI(frac_bits);
|
|
}
|
|
return fixed_atan2(-y, -x, frac_bits) - FIXED_PI(frac_bits);
|
|
}
|
|
|
|
shift = 30 - frac_bits;
|
|
if (shift >= 0)
|
|
{
|
|
x_q30 = (int64_t)x << shift;
|
|
y_q30 = (int64_t)y << shift;
|
|
}
|
|
else
|
|
{
|
|
x_q30 = (int64_t)x >> (-shift);
|
|
y_q30 = (int64_t)y >> (-shift);
|
|
}
|
|
|
|
for (size_t i = 0; i < sizeof(s_fixed_atan_table_q30) / sizeof(s_fixed_atan_table_q30[0]); ++i)
|
|
{
|
|
int64_t x_next;
|
|
int64_t y_next;
|
|
|
|
if (y_q30 > 0)
|
|
{
|
|
x_next = x_q30 + (y_q30 >> i);
|
|
y_next = y_q30 - (x_q30 >> i);
|
|
angle_q30 += s_fixed_atan_table_q30[i];
|
|
}
|
|
else
|
|
{
|
|
x_next = x_q30 - (y_q30 >> i);
|
|
y_next = y_q30 + (x_q30 >> i);
|
|
angle_q30 -= s_fixed_atan_table_q30[i];
|
|
}
|
|
|
|
x_q30 = x_next;
|
|
y_q30 = y_next;
|
|
}
|
|
|
|
if (shift >= 0)
|
|
{
|
|
return (int32_t)(angle_q30 >> shift);
|
|
}
|
|
return (int32_t)(angle_q30 << (-shift));
|
|
}
|
|
|
|
int32_t fixed_asin(int32_t value, uint8_t frac_bits)
|
|
{
|
|
const int32_t one = FIXED_ONE(frac_bits);
|
|
const int32_t clamped = FIXED_CLAMP(value, -one, one);
|
|
const int32_t one_minus_square = one - FIXED_MUL(clamped, clamped, frac_bits);
|
|
|
|
if (one_minus_square <= 0)
|
|
{
|
|
return clamped >= 0 ? FIXED_HALF_PI(frac_bits) : -FIXED_HALF_PI(frac_bits);
|
|
}
|
|
|
|
return fixed_atan2(clamped, fixed_sqrt(one_minus_square, frac_bits), frac_bits);
|
|
}
|