101 lines
1.9 KiB
C
101 lines
1.9 KiB
C
#include <stdint.h>
|
|
#ifndef FIXED_POINT_H
|
|
#define FIXED_POINT_H
|
|
|
|
typedef struct
|
|
{
|
|
int32_t raw;
|
|
} fp32_t;
|
|
|
|
/* Fixed Point Arithmetic bit count constants */
|
|
#define NUM_FRAC_BITS 14
|
|
#define NUM_INT_BITS (31 - NUM_FRAC_BITS)
|
|
#define CONVERSION_FACTOR (1 << NUM_FRAC_BITS) /* f = 2^q, (2^14) */
|
|
|
|
/* Fixed Point Arithmetic conversion operations */
|
|
/* Converts an integer n to a fixed point number */
|
|
inline fp32_t
|
|
fp_from_int (int32_t n)
|
|
{
|
|
return (fp32_t){ n * CONVERSION_FACTOR };
|
|
}
|
|
|
|
/* Handles conversion of fixed point to integer,
|
|
with truncation */
|
|
inline int32_t
|
|
fp_floor (fp32_t x)
|
|
{
|
|
return x.raw / CONVERSION_FACTOR;
|
|
}
|
|
|
|
/* Handles conversion of fixed point to integer,
|
|
with rounding */
|
|
inline int32_t
|
|
fp_round (fp32_t x)
|
|
{
|
|
if (x.raw >= 0)
|
|
return (x.raw + CONVERSION_FACTOR / 2) / CONVERSION_FACTOR;
|
|
else
|
|
return (x.raw - CONVERSION_FACTOR / 2) / CONVERSION_FACTOR;
|
|
}
|
|
|
|
/* Add two fixed points */
|
|
inline fp32_t
|
|
fp_add (fp32_t x, fp32_t y)
|
|
{
|
|
return (fp32_t){ x.raw + y.raw };
|
|
}
|
|
|
|
/* Subtract two fixed points */
|
|
inline fp32_t
|
|
fp_sub (fp32_t x, fp32_t y)
|
|
{
|
|
return (fp32_t){ x.raw - y.raw };
|
|
}
|
|
|
|
|
|
|
|
/* Multiple two fixed points */
|
|
inline fp32_t
|
|
fp_mul (fp32_t x, fp32_t y)
|
|
{
|
|
return (fp32_t){ ((int64_t)x.raw) * y.raw / CONVERSION_FACTOR };
|
|
}
|
|
|
|
/* Divide two fixed points */
|
|
inline fp32_t
|
|
fp_div (fp32_t x, fp32_t y)
|
|
{
|
|
return (fp32_t){ ((int64_t)x.raw) * CONVERSION_FACTOR / y.raw };
|
|
}
|
|
|
|
/* Multiply fixed point and integer */
|
|
inline fp32_t
|
|
fp_mul_int (fp32_t x, int32_t n)
|
|
{
|
|
return (fp32_t){ x.raw * n };
|
|
}
|
|
|
|
/* Divide fixed point by integer */
|
|
inline fp32_t
|
|
fp_div_int (fp32_t x, int32_t n)
|
|
{
|
|
return (fp32_t){ x.raw / n };
|
|
}
|
|
|
|
/* Add fixed point to integer */
|
|
inline fp32_t
|
|
fp_add_int (fp32_t x, int32_t n)
|
|
{
|
|
return (fp32_t){ x.raw + n * CONVERSION_FACTOR };
|
|
}
|
|
|
|
/* Subtract integer from fixed point */
|
|
inline fp32_t
|
|
fp_sub_int (fp32_t x, int32_t n)
|
|
{
|
|
return (fp32_t){ x.raw - n * CONVERSION_FACTOR };
|
|
}
|
|
|
|
#endif //FIXED_POINT_H
|