257 lines
6.0 KiB
C
257 lines
6.0 KiB
C
/**
|
|
* Copyright (c) 2025 Dominic Masters
|
|
*
|
|
* This software is released under the MIT License.
|
|
* https://opensource.org/licenses/MIT
|
|
*/
|
|
|
|
#include "fixed.h"
|
|
#include "assert/assert.h"
|
|
|
|
float_t fx248Fromi32(const int32_t b) {
|
|
return (float_t)b << FIXED248_FRACTION_BITS;
|
|
}
|
|
|
|
float_t fx248Fromu32(const uint32_t b) {
|
|
return (float_t)((int32_t)b << FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
float_t fx248Fromf32(const float_t b) {
|
|
return (float_t)(b * (1 << FIXED248_FRACTION_BITS));
|
|
}
|
|
|
|
float_t fx248Fromu16(const uint16_t b) {
|
|
return (float_t)((int32_t)b << FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
float_t fx248Fromu8(const uint8_t b) {
|
|
return (float_t)((int32_t)b << FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
|
|
|
|
int32_t fx248Toi32(const float_t a) {
|
|
return a >> FIXED248_FRACTION_BITS;
|
|
}
|
|
|
|
uint32_t fx248Tou32(const float_t a) {
|
|
return (uint32_t)(a >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
float_t fx248Tof32(const float_t a) {
|
|
return (float_t)a / (1 << FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
uint16_t fx248Tou16(const float_t a) {
|
|
return (uint16_t)(a >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
uint8_t fx248Tou8(const float_t a) {
|
|
return (uint8_t)(a >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Addfx248(const float_t a, const float_t b) {
|
|
return a + b;
|
|
}
|
|
|
|
float_t fx248Addi32(const float_t a, const int32_t b) {
|
|
return fx248Addfx248(a, fx248Fromi32(b));
|
|
}
|
|
|
|
float_t fx248Addu32(const float_t a, const uint32_t b) {
|
|
return fx248Addfx248(a, fx248Fromu32(b));
|
|
}
|
|
|
|
float_t fx248Addf32(const float_t a, const float_t b) {
|
|
return fx248Addfx248(a, fx248Fromf32(b));
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Subfx248(const float_t a, const float_t b) {
|
|
return a - b;
|
|
}
|
|
|
|
float_t fx248Subi32(const float_t a, const int32_t b) {
|
|
return fx248Subfx248(a, fx248Fromi32(b));
|
|
}
|
|
|
|
float_t fx248Subu32(const float_t a, const uint32_t b) {
|
|
return fx248Subfx248(a, fx248Fromu32(b));
|
|
}
|
|
|
|
float_t fx248Subf32(const float_t a, const float_t b) {
|
|
return fx248Subfx248(a, fx248Fromf32(b));
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Mulfx248(const float_t a, const float_t b) {
|
|
return (float_t)(((int64_t)a * (int64_t)b) >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
float_t fx248Muli32(const float_t a, const int32_t b) {
|
|
return (float_t)(((int64_t)a * (int64_t)b) >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
float_t fx248Mulu32(const float_t a, const uint32_t b) {
|
|
return (float_t)(
|
|
((int64_t)a * (int64_t)(int32_t)b
|
|
) >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
float_t fx248Mulf32(const float_t a, const float_t b) {
|
|
return (float_t)((
|
|
(int64_t)a * (int64_t)(b * (1 << FIXED248_FRACTION_BITS))
|
|
) >> FIXED248_FRACTION_BITS);
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Divfx248(const float_t a, const float_t b) {
|
|
assertFalse(b == 0, "Division by zero in fx248Divfx248");
|
|
return (float_t)(((int64_t)a << FIXED248_FRACTION_BITS) / (int64_t)b);
|
|
}
|
|
|
|
float_t fx248Divi32(const float_t a, const int32_t b) {
|
|
assertFalse(b == 0, "Division by zero in fx248Divi32");
|
|
return (float_t)(((int64_t)a << FIXED248_FRACTION_BITS) / (int64_t)b);
|
|
}
|
|
|
|
float_t fx248Divu32(const float_t a, const uint32_t b) {
|
|
assertFalse(b == 0, "Division by zero in fx248Divu32");
|
|
return (float_t)(
|
|
((int64_t)a << FIXED248_FRACTION_BITS
|
|
) / (int64_t)(int32_t)b);
|
|
}
|
|
|
|
float_t fx248Divf32(const float_t a, const float_t b) {
|
|
assertFalse(b == 0, "Division by zero in fx248Divf32");
|
|
return (float_t)((
|
|
(int64_t)a << FIXED248_FRACTION_BITS
|
|
) / (int64_t)(b * (1 << FIXED248_FRACTION_BITS)));
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Floor(const float_t a) {
|
|
return a & ~((1 << FIXED248_FRACTION_BITS) - 1);
|
|
}
|
|
|
|
float_t fx248Ceil(const float_t a) {
|
|
if(a & ((1 << FIXED248_FRACTION_BITS) - 1)) {
|
|
return (a & ~((1 << FIXED248_FRACTION_BITS) - 1)) + (1 << FIXED248_FRACTION_BITS);
|
|
}
|
|
return a;
|
|
}
|
|
|
|
float_t fx248Round(const float_t a) {
|
|
if(a & ((1 << (FIXED248_FRACTION_BITS - 1)) - 1)) {
|
|
return (a & ~((1 << FIXED248_FRACTION_BITS) - 1)) + (1 << FIXED248_FRACTION_BITS);
|
|
}
|
|
return a & ~((1 << FIXED248_FRACTION_BITS) - 1);
|
|
}
|
|
|
|
|
|
|
|
uint32_t fx248Flooru32(const float_t a) {
|
|
return (uint32_t)((a >> FIXED248_FRACTION_BITS) & 0xFFFFFFFF);
|
|
}
|
|
|
|
uint32_t fx248Ceilu32(const float_t a) {
|
|
return (uint32_t)(((a + ((1 << FIXED248_FRACTION_BITS) - 1)) >> FIXED248_FRACTION_BITS) & 0xFFFFFFFF);
|
|
}
|
|
|
|
uint32_t fx248Roundu32(const float_t a) {
|
|
return (uint32_t)(((a + (1 << (FIXED248_FRACTION_BITS - 1))) >> FIXED248_FRACTION_BITS) & 0xFFFFFFFF);
|
|
}
|
|
|
|
float_t fx248Sqrt(const float_t a) {
|
|
if(a == 0) return 0;
|
|
|
|
float_t y = a > FIXED248(1, 0) ? a : FIXED248(1, 0);
|
|
float_t last = 0;
|
|
int max_iter = 16;
|
|
while(y != last && max_iter-- > 0) {
|
|
last = y;
|
|
int32_t div = (int32_t)(((int64_t)a << FIXED248_FRACTION_BITS) / y);
|
|
y = (y + div) >> 1;
|
|
}
|
|
return y;
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Max(const float_t a, const float_t b) {
|
|
return (a > b) ? a : b;
|
|
}
|
|
|
|
float_t fx248Min(const float_t a, const float_t b) {
|
|
return (a < b) ? a : b;
|
|
}
|
|
|
|
float_t fx248Clamp(
|
|
const float_t a,
|
|
const float_t min,
|
|
const float_t max
|
|
) {
|
|
return (a < min) ? min : (a > max) ? max : a;
|
|
}
|
|
|
|
float_t fx248Abs(const float_t a) {
|
|
return (a < 0) ? -a : a;
|
|
}
|
|
|
|
|
|
|
|
float_t fx248Atan2(
|
|
const float_t y,
|
|
const float_t x
|
|
) {
|
|
// Handle special cases
|
|
if (x == 0) {
|
|
if (y > 0) return FX248_HALF_PI;
|
|
if (y < 0) return -FX248_HALF_PI;
|
|
return 0;
|
|
}
|
|
|
|
// Use absolute values for quadrant correction
|
|
float_t abs_y = y;
|
|
if (abs_y < 0) abs_y = -abs_y;
|
|
|
|
float_t angle;
|
|
if (abs_y < fx248Abs(x)) {
|
|
float_t z = fx248Divfx248(y, x);
|
|
float_t z2 = fx248Mulfx248(z, z);
|
|
float_t z3 = fx248Mulfx248(z2, z);
|
|
float_t z5 = fx248Mulfx248(z3, z2);
|
|
angle = fx248Subfx248(
|
|
fx248Addfx248(z, fx248Divfx248(z5, fx248Fromi32(5))),
|
|
fx248Divfx248(z3, fx248Fromi32(3))
|
|
);
|
|
if (x < 0) {
|
|
if (y < 0) {
|
|
angle -= FX248_PI;
|
|
} else {
|
|
angle += FX248_PI;
|
|
}
|
|
}
|
|
} else {
|
|
float_t z = fx248Divfx248(x, y);
|
|
float_t z2 = fx248Mulfx248(z, z);
|
|
float_t z3 = fx248Mulfx248(z2, z);
|
|
float_t z5 = fx248Mulfx248(z3, z2);
|
|
angle = fx248Subfx248(
|
|
fx248Addfx248(z, fx248Divfx248(z5, fx248Fromi32(5))),
|
|
fx248Divfx248(z3, fx248Fromi32(3))
|
|
);
|
|
if (y > 0) {
|
|
angle = FX248_HALF_PI - angle;
|
|
} else {
|
|
angle = -FX248_HALF_PI - angle;
|
|
}
|
|
}
|
|
return angle;
|
|
} |