78 lines
1.8 KiB
C
78 lines
1.8 KiB
C
|
#include "types.h"
|
||
|
|
||
|
#ifndef FIXED_H
|
||
|
#define FIXED_H
|
||
|
|
||
|
#define FIX_SHIFT 8
|
||
|
#define HALF_FIX_SHIFT 4
|
||
|
#define FIX_SCALE ( 1 << FIX_SHIFT )
|
||
|
#define FIX_SCALE_FLOAT ((float)(FIX_SCALE))
|
||
|
|
||
|
typedef s32 FIXED;
|
||
|
|
||
|
static inline FIXED fixed_OverMultiply(FIXED a, FIXED b) {
|
||
|
// This should multiply two fixed-point numbers sacrificing a little
|
||
|
// accuracy in exchange for less chance of an overflow
|
||
|
|
||
|
return ((a >> HALF_FIX_SHIFT) * (b >> HALF_FIX_SHIFT));
|
||
|
}
|
||
|
|
||
|
static inline FIXED fixed_multiply (FIXED a, FIXED b) {
|
||
|
// Multiply two fixed numbers. Possibility of overflow.
|
||
|
return (a * b) >> FIX_SHIFT;
|
||
|
}
|
||
|
|
||
|
static inline FIXED fixed_divide(FIXED a, FIXED b) {
|
||
|
// Divide two fixed point numbers
|
||
|
if (b != 0){
|
||
|
return (a * FIX_SCALE) / b;
|
||
|
}
|
||
|
else {
|
||
|
// Return a large number if division by zero
|
||
|
// Hopefully this won't break anything :P
|
||
|
return 100000 << FIX_SHIFT;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
static inline FIXED float_to_fixed(float a) {
|
||
|
// Convert a float to fixed point
|
||
|
return ((FIXED)(a * FIX_SCALE_FLOAT));
|
||
|
}
|
||
|
|
||
|
static inline float fixed_to_float(FIXED a) {
|
||
|
// Convert fixed point to float
|
||
|
return (a / FIX_SCALE_FLOAT);
|
||
|
}
|
||
|
|
||
|
static inline void swapFixed(FIXED *a, FIXED *b) {
|
||
|
// Swap two fixed point integer pointers
|
||
|
FIXED temp;
|
||
|
temp = *a;
|
||
|
*a = *b;
|
||
|
*b = temp;
|
||
|
}
|
||
|
|
||
|
static inline FIXED fixed_sqrt(FIXED a, int iterations) {
|
||
|
// Calculate square root of a fixed-point number using Binary-Search
|
||
|
FIXED low = 0;
|
||
|
FIXED high = a;
|
||
|
FIXED mid;
|
||
|
FIXED midSquared;
|
||
|
for (int i = 0; i < iterations; i++) {
|
||
|
mid = fixed_divide((low + high), 2 << FIX_SHIFT);
|
||
|
midSquared = fixed_OverMultiply(mid, mid);
|
||
|
if (midSquared == a) {
|
||
|
return mid;
|
||
|
}
|
||
|
else if (midSquared > a) {
|
||
|
high = mid;
|
||
|
}
|
||
|
else {
|
||
|
low = mid;
|
||
|
}
|
||
|
}
|
||
|
return mid;
|
||
|
}
|
||
|
|
||
|
#endif // FIXED_H
|