1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
|
#ifndef __FP_MATH_H__
#define __FP_MATH_H__
#define FP_SHIFT 10
#define ROUND_BIT (FP_SHIFT - 1)
#define ONE FP(1)
#define _fp(x) ((fp_t) {x})
static const fp_t FP_ZERO = {.val = 0};
static const fp_t FP_ONE = {.val = (1 << FP_SHIFT)};
static inline fpbuf_t _point(fp_t x)
{
return (x.val % (1 << FP_SHIFT));
}
#define fp2str(x) x.val
/*(x.val >> FP_SHIFT), (x.val % (1 << FP_SHIFT)) */
#define _FP_ "%ld/1024"
static inline fp_t FP(fpbuf_t x)
{
return _fp(((fpbuf_t) x) << FP_SHIFT);
}
static inline fpbuf_t _floor(fp_t x)
{
return x.val >> FP_SHIFT;
}
/* FIXME: negative rounding */
static inline fpbuf_t _round(fp_t x)
{
return _floor(x) + ((x.val >> ROUND_BIT) & 1);
}
/* divide two integers to obtain a fixed point value */
static inline fp_t _frac(fpbuf_t a, fpbuf_t b)
{
return _fp(FP(a).val / (b));
}
/* multiply two fixed point values */
static inline fp_t _mul(fp_t a, fp_t b)
{
return _fp((a.val * b.val) >> FP_SHIFT);
}
static inline fp_t _div(fp_t a, fp_t b)
{
/* try not to overflow */
if (unlikely( a.val > (2l << (BITS_PER_LONG - FP_SHIFT)) ))
return _fp((a.val / b.val) << FP_SHIFT);
else
return _fp((a.val << FP_SHIFT) / b.val);
}
static inline fp_t _add(fp_t a, fp_t b)
{
return _fp(a.val + b.val);
}
static inline fp_t _sub(fp_t a, fp_t b)
{
return _fp(a.val - b.val);
}
static inline fp_t _neg(fp_t x)
{
return _fp(-x.val);
}
static inline fp_t _abs(fp_t x)
{
return _fp(abs(x.val));
}
/* equiv. to casting float/double to integer */
static inline fpbuf_t _fp_to_integer(fp_t x)
{
return _floor(_abs(x)) * ((x.val > 0) ? 1 : -1);
}
static inline fp_t _integer_to_fp(fpbuf_t x)
{
return _frac(x,1);
}
static inline int _leq(fp_t a, fp_t b)
{
return a.val <= b.val;
}
static inline int _geq(fp_t a, fp_t b)
{
return a.val >= b.val;
}
static inline int _lt(fp_t a, fp_t b)
{
return a.val < b.val;
}
static inline int _gt(fp_t a, fp_t b)
{
return a.val > b.val;
}
static inline int _eq(fp_t a, fp_t b)
{
return a.val == b.val;
}
static inline fp_t _max(fp_t a, fp_t b)
{
if (a.val < b.val)
return b;
else
return a;
}
#endif
|
