80 lines
1.6 KiB
C
80 lines
1.6 KiB
C
#include "fixed.h"
|
|
|
|
fixed_t int_to_fixed(i32 i) { return i << 16; }
|
|
|
|
i32 fixed_to_int(fixed_t f) { return f >> 16; }
|
|
|
|
fixed_t float_to_fixed(f32 f) { return (fixed_t)(f * 65536.0f); }
|
|
|
|
f32 fixed_to_float(fixed_t f) { return (f32)f / 65536.0f; }
|
|
|
|
fixed_t fixed_add(fixed_t a, fixed_t b) { return a + b; }
|
|
|
|
fixed_t fixed_sub(fixed_t a, fixed_t b) { return a - b; }
|
|
|
|
fixed_t fixed_mul(fixed_t a, fixed_t b) {
|
|
i32 a_hi = a >> 16;
|
|
u32 a_lo = (u32)a & 0xFFFFU;
|
|
i32 b_hi = b >> 16;
|
|
u32 b_lo = (u32)b & 0xFFFFU;
|
|
|
|
i32 p0 = (i32)(a_lo * b_lo) >> 16;
|
|
i32 p1 = a_hi * (i32)b_lo;
|
|
i32 p2 = (i32)a_lo * b_hi;
|
|
i32 p3 = (a_hi * b_hi) << 16;
|
|
|
|
return p0 + p1 + p2 + p3;
|
|
}
|
|
|
|
fixed_t fixed_div(fixed_t a, fixed_t b) {
|
|
i32 negative;
|
|
u32 ua, ub, quotient, remainder;
|
|
i32 i;
|
|
|
|
if (b == 0)
|
|
return 0;
|
|
|
|
negative = ((a < 0) ^ (b < 0));
|
|
|
|
ua = (a < 0) ? -a : a;
|
|
ub = (b < 0) ? -b : b;
|
|
|
|
quotient = 0;
|
|
remainder = 0;
|
|
|
|
for (i = 0; i < 32; i++) {
|
|
remainder <<= 1;
|
|
if (ua & 0x80000000U) {
|
|
remainder |= 1;
|
|
}
|
|
ua <<= 1;
|
|
|
|
if (remainder >= ub) {
|
|
remainder -= ub;
|
|
quotient |= 1;
|
|
}
|
|
|
|
if (i < 31) {
|
|
quotient <<= 1;
|
|
}
|
|
}
|
|
|
|
return negative ? -(i32)quotient : (i32)quotient;
|
|
}
|
|
|
|
i32 fixed_eq(fixed_t a, fixed_t b) { return a == b; }
|
|
|
|
i32 fixed_ne(fixed_t a, fixed_t b) { return a != b; }
|
|
|
|
i32 fixed_lt(fixed_t a, fixed_t b) { return a < b; }
|
|
|
|
i32 fixed_le(fixed_t a, fixed_t b) { return a <= b; }
|
|
|
|
i32 fixed_gt(fixed_t a, fixed_t b) { return a > b; }
|
|
|
|
i32 fixed_ge(fixed_t a, fixed_t b) { return a >= b; }
|
|
|
|
fixed_t fixed_neg(fixed_t f) { return -f; }
|
|
|
|
fixed_t fixed_abs(fixed_t f) { return (f < 0) ? -f : f; }
|