1 /*
2 Copyright (c) 2010, Warmux Team
3
4 Large parts of the code are from the fixed point library of Markus Trenkwalder
5 Copyright (c) 2007, Markus Trenkwalder
6
7 All rights reserved.
8
9 Redistribution and use in source and binary forms, with or without
10 modification, are permitted provided that the following conditions are met:
11
12 * Redistributions of source code must retain the above copyright notice,
13 this list of conditions and the following disclaimer.
14
15 * Redistributions in binary form must reproduce the above copyright notice,
16 this list of conditions and the following disclaimer in the documentation
17 and/or other materials provided with the distribution.
18
19 * Neither the name of the library's copyright owner nor the names of its
20 contributors may be used to endorse or promote products derived from this
21 software without specific prior written permission.
22
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
26 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
27 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
28 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
29 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
30 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
31 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
32 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
33 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 #ifndef FIXEDP_CLASS_H_INCLUDED
37 #define FIXEDP_CLASS_H_INCLUDED
38
39 #ifdef _MSC_VER
40 #pragma once
41 #endif
42
43 #include "fixed_func.h"
44 #include <sstream>
45 #include <stdio.h>
46 #include <limits.h>
47
48 //#define TRACK_MINMAX
49
50 namespace fp {
51
52 // The template argument p in all of the following functions refers to the
53 // fixed point precision (e.g. p = 8 gives 24.8 fixed point functions).
54 template <int p>
55 struct fixed_point {
56 fint_t intValue;
57 #ifdef TRACK_MINMAX
58 static fint_t min, max;
~fixed_pointfixed_point59 ~fixed_point()
60 {
61 if (min > intValue) {
62 min = intValue;
63 printf("Min: %"PRIi64"\n", min);
64 }
65 if (max < intValue) {
66 max = intValue;
67 printf("Max: %"PRIi64"\n", max);
68 }
69 }
70 #endif
71
fixed_pointfixed_point72 fixed_point() {}
fixed_pointfixed_point73 /*explicit*/ fixed_point(fint_t i) : intValue(i << p) {}
74 #if FIXINT_BITS == 64
fixed_pointfixed_point75 /*explicit*/ fixed_point(int32_t i) : intValue(((fint_t)i) << p) {}
76 #endif
fixed_pointfixed_point77 /*explicit*/ fixed_point(float f) : intValue(float2fix<p>(f)) {}
fixed_pointfixed_point78 /*explicit*/ fixed_point(double d) : intValue(float2fix<p>((float)d)) {}
fixed_pointfixed_point79 /*explicit*/ fixed_point(unsigned int u) : intValue(((fint_t)u) << p) {}
fixed_pointfixed_point80 /*explicit*/ fixed_point(unsigned long int u) : intValue(((fint_t)u) << p) {}
81
82 fixed_point& operator += (const fixed_point& r) { intValue += r.intValue; return *this; }
83 fixed_point& operator -= (const fixed_point& r) { intValue -= r.intValue; return *this; }
84 fixed_point& operator *= (const fixed_point& r) { intValue = fixmul<p>(intValue, r.intValue); return *this; }
85 fixed_point& operator /= (const fixed_point& r) { intValue = fixdiv<p>(intValue, r.intValue); return *this; }
86
87 fixed_point& operator *= (int32_t r) { intValue *= r; return *this; }
88 fixed_point& operator /= (int32_t r) { intValue /= r; return *this; }
89
90 fixed_point operator - () const { fixed_point x; x.intValue = -intValue; return x; }
91 fixed_point operator + (const fixed_point& r) const { fixed_point x = *this; x += r; return x;}
92 fixed_point operator - (const fixed_point& r) const { fixed_point x = *this; x -= r; return x;}
93 fixed_point operator * (const fixed_point& r) const { fixed_point x = *this; x *= r; return x;}
94 fixed_point operator / (const fixed_point& r) const { fixed_point x = *this; x /= r; return x;}
95
96 bool operator == (const fixed_point& r) const { return intValue == r.intValue; }
97 bool operator == (int i) const { return intValue == (((fint_t)i) << p); }
98 bool operator != (const fixed_point& r) const { return !(*this == r); }
99 bool operator < (const fixed_point& r) const { return intValue < r.intValue; }
100 bool operator < (int i) const { return intValue < (((fint_t)i) << p); }
101 bool operator > (const fixed_point& r) const { return intValue > r.intValue; }
102 bool operator > (int i) const { return intValue > (((fint_t)i) << p); }
103 bool operator <= (const fixed_point& r) const { return intValue <= r.intValue; }
104 bool operator <= (int i) const { return intValue <= (((fint_t)i) << p); }
105 bool operator >= (const fixed_point& r) const { return intValue >= r.intValue; }
106 bool operator >= (int i) const { return intValue >= (((fint_t)i) << p); }
107
108 fixed_point operator + (int32_t r) const { fixed_point x = *this; x += r; return x;}
109 fixed_point operator - (int32_t r) const { fixed_point x = *this; x -= r; return x;}
110 fixed_point operator * (int32_t r) const { fixed_point x = *this; x *= r; return x;}
111 fixed_point operator / (int32_t r) const { fixed_point x = *this; x /= r; return x;}
112 fixed_point operator + (unsigned int r) const { fixed_point x = *this; x += r; return x;}
113 fixed_point operator - (unsigned int r) const { fixed_point x = *this; x -= r; return x;}
114 fixed_point operator * (unsigned int r) const { fixed_point x = *this; x *= r; return x;}
115 fixed_point operator / (unsigned int r) const { fixed_point x = *this; x /= r; return x;}
116
117 operator int() const
118 {
119 fuint_t sign = ((fuint_t)intValue)>>(FIXINT_BITS-1);
120 return int(((fint_t)(intValue+(sign<<p)-sign))>>p);
121 }
122
123 // Must be used explicily as we don't want to calculate with doubles!
toDoublefixed_point124 double toDouble() const
125 {
126 static const double factor = 1.0 / (double)(1 << p);
127 return intValue * factor;
128 }
tofloatfixed_point129 float tofloat() const
130 {
131 static const float factor = 1.0f / (float)(1 << p);
132 return intValue * factor;
133 }
134
135 // Warning, this tests strict equality!
IsNotZerofixed_point136 bool IsNotZero() const { return intValue; }
137 };
138
139 // Specializations for use with plain integers
140 template <int p>
141 inline fixed_point<p> operator + (int32_t a, fixed_point<p> b)
142 { return b + a; }
143
144 template <int p>
145 inline fixed_point<p> operator - (int32_t a, fixed_point<p> b)
146 { return -b + a; }
147
148 template <int p>
149 inline fixed_point<p> operator * (int32_t a, fixed_point<p> b)
150 { return b * a; }
151
152 template <int p>
153 inline fixed_point<p> operator / (int32_t a, const fixed_point<p>& b)
154 { fixed_point<p> r(a); r /= b; return r; }
155 #ifdef TRACK_MINMAX
156 template <int p>
157 fint_t fixed_point<p>::min = LLONG_MAX;
158 template <int p>
159 fint_t fixed_point<p>::max = LLONG_MIN;
160 #endif
161
162
163 template <int p>
164 inline fixed_point<p> operator + (unsigned int a, const fixed_point<p>& b)
165 { return b + a; }
166
167 template <int p>
168 inline fixed_point<p> operator - (unsigned int a, const fixed_point<p>& b)
169 { return -b + a; }
170
171 template <int p>
172 inline fixed_point<p> operator * (unsigned int a, const fixed_point<p>& b)
173 { return b * a; }
174
175 template <int p>
176 inline fixed_point<p> operator / (unsigned int a, const fixed_point<p>& b)
177 { fixed_point<p> r(a); r /= b; return r; }
178
179 #ifdef SIZE_T_fp_METHODS
180 template <int p>
181 inline fixed_point<p> operator + (size_t a, fixed_point<p> b)
182 { return b + a; }
183
184 template <int p>
185 inline fixed_point<p> operator - (size_t a, fixed_point<p> b)
186 { return -b + a; }
187
188 template <int p>
189 inline fixed_point<p> operator * (size_t a, fixed_point<p> b)
190 { return b * a; }
191
192 template <int p>
193 inline fixed_point<p> operator / (size_t a, fixed_point<p> b)
194 { fixed_point<p> r(a); r /= b; return r; }
195 #endif
196
197 template<int p>
198 inline fixed_point<p> round(fixed_point<p> r);
199 template<>
round(fixed_point<16> r)200 inline fixed_point<16> round(fixed_point<16> r)
201 {
202 // Very important to have it this way: 0 would be rounded to -1 otherwise
203 r.intValue += (r.intValue<0) ? -32768 : 32768; // (1<<(16-1))
204 r.intValue &= 0xFFFFFFFFFFFF0000LL;
205 return r;
206 }
207
208 template<int p>
209 inline int uround(const fixed_point<p>& r);
210 template<>
uround(const fixed_point<16> & r)211 inline int uround(const fixed_point<16>& r)
212 {
213 return int((r.intValue + 32768)>>16);
214 }
215
216 namespace detail {
isDigit(int c)217 static bool isDigit(int c) {
218 return c >= '0' && c <= '9';
219 }
220 }
221
222 template <int p>
223 std::istream & operator>> (std::istream & is, fixed_point<p> & r)
224 {
225 std::stringstream buffer;
226 int next = is.peek();
227 bool minus = (next == '-');
228 if (minus) {
229 buffer << "-";
230 is.get();// skip one character
231 }
232 buffer << '0'; // assuming a leading zero makes it possible to parse numbers starting with a dot and parses "" as 0.
233 next = is.peek();
234 while (next != (int)is.eof() && detail::isDigit(next)) {
235 char c = is.get();
236 buffer << c;
237 next = is.peek();
238 }
239 int number;
240 buffer >> number;
241 r = number;
242 next = is.peek();
243 if (next == '.') {
244 is.get();// skip point character
245 next = is.peek();
246 fixed_point<p> factor = 1;
247 fixed_point<p> ten = 10;
248 while (next != (int)is.eof() && detail::isDigit(next)) {
249 factor /= ten;
250 char c;
251 is >> c;
252 int digit = c - '0';
253 if (minus)
254 r -= digit * factor;
255 else
256 r += digit * factor;
257 buffer << c;
258 next = is.peek();
259 }
260 }
261
262 return is;
263 }
264
265
266 template <int p>
267 void printTo(std::ostream & os, const fixed_point<p> & r, int digits = -1)
268 {
269 if (digits == -1)
270 digits = p>>2;
271 fixed_point<p> rounding_correction = 0.5;
272 fixed_point<p> ten = 10;
273 for (int k = 0; k < digits; k++) {
274 rounding_correction = rounding_correction / ten;
275 }
276 if (r < 0)
277 rounding_correction = - rounding_correction;
278 fixed_point<p> to_print = r + rounding_correction;
279
280 fint_t left_of_dot = (int) r;
281 os << left_of_dot;
282 if (digits == 0) {
283 return;
284 }
285 os << ".";
286
287 to_print -= left_of_dot;
288 fixed_point<p> zero = 0;
289
290 int zeros = 0;
291 if (to_print < 0) {
292 to_print = - to_print;
293 }
294 for (int i=0; i<digits; i++) {
295 to_print *= 10;
296 int digit = (int)to_print;
297 to_print = to_print - digit;
298 if (digit == 0) {
299 zeros++;
300 } else {
301 for (int j=0; j<zeros; j++) {
302 os << "0";
303 }
304 zeros = 0;
305 os << digit;
306 }
307 }
308 // show at least one digit after the point:
309 if (zeros == digits) {
310 os << "0";
311 }
312 }
313
314 template <int p>
315 std::ostream & operator<< (std::ostream & os, const fixed_point<p> & r)
316 {
317 printTo(os, r);
318 return os;
319 }
320
321 // math functions
322 // no default implementation
323
324 template <int p>
325 inline fixed_point<p> sin(fixed_point<p> a);
326
327 template <int p>
328 inline fixed_point<p> cos(fixed_point<p> a);
329
330 template <int p>
331 inline fixed_point<p> acos(fixed_point<p> a);
332
333 template <int p>
334 inline fixed_point<p> asin(fixed_point<p> a);
335
336 template <int p>
337 inline fixed_point<p> atan(fixed_point<p> a);
338
339 template <int p>
340 inline fixed_point<p> sqrt(fixed_point<p> a);
341
342 template <int p>
343 inline fixed_point<p> sqrt_approx(fixed_point<p> a);
344
345 template <int p>
346 inline fixed_point<p> rsqrt(fixed_point<p> a);
347
348 template <int p>
349 inline fixed_point<p> inv(fixed_point<p> a);
350
351 template <int p>
abs(fixed_point<p> a)352 inline fixed_point<p> abs(fixed_point<p> a)
353 {
354 fint_t sign = a.intValue >> (FIXINT_BITS-1);
355 fixed_point<p> r;
356 r.intValue = a.intValue^sign;
357 r.intValue -= sign;
358 return r;
359 }
360
361 template <int p>
fmod(fixed_point<p> numerator,fixed_point<p> denominator)362 inline fixed_point<p> fmod(fixed_point<p> numerator, fixed_point<p> denominator)
363 {
364 return numerator - denominator * floor(numerator / denominator) ;
365 }
366
367 template <int p>
floor(fixed_point<p> a)368 inline fixed_point<p> floor(fixed_point<p> a)
369 {
370 fixed_point<p> r = a.intValue >> p;
371 return r;
372 }
373
374 // specializations for 16.16 format
375
376 template <>
sin(fixed_point<16> a)377 inline fixed_point<16> sin(fixed_point<16> a)
378 {
379 fixed_point<16> r;
380 r.intValue = fixsin16(a.intValue);
381 return r;
382 }
383
384 template <>
cos(fixed_point<16> a)385 inline fixed_point<16> cos(fixed_point<16> a)
386 {
387 fixed_point<16> r;
388 r.intValue = fixcos16(a.intValue);
389 return r;
390 }
391
392 template <>
acos(fixed_point<16> a)393 inline fixed_point<16> acos(fixed_point<16> a)
394 {
395 fixed_point<16> r;
396 r.intValue = fixacos16(a.intValue);
397 return r;
398 }
399
400 template <>
asin(fixed_point<16> a)401 inline fixed_point<16> asin(fixed_point<16> a)
402 {
403 fixed_point<16> r;
404 r.intValue = fixasin16(a.intValue);
405 return r;
406 }
407
408 template <>
atan(fixed_point<16> a)409 inline fixed_point<16> atan(fixed_point<16> a)
410 {
411 fixed_point<16> r;
412 r.intValue = fixatan16(a.intValue);
413 return r;
414 }
415
416 template <>
sqrt(fixed_point<16> a)417 inline fixed_point<16> sqrt(fixed_point<16> a)
418 {
419 fixed_point<16> r;
420 r.intValue = fixsqrt16(a.intValue);
421 return r;
422 }
423
424 template <>
sqrt_approx(fixed_point<16> a)425 inline fixed_point<16> sqrt_approx(fixed_point<16> a)
426 {
427 fixed_point<16> r;
428 r.intValue = fixsqrt16_approx(a.intValue);
429 return r;
430 }
431
432 template <>
rsqrt(fixed_point<16> a)433 inline fixed_point<16> rsqrt(fixed_point<16> a)
434 {
435 fixed_point<16> r;
436 r.intValue = fixrsqrt16(a.intValue);
437 return r;
438 }
439
440 template <>
inv(fixed_point<16> a)441 inline fixed_point<16> inv(fixed_point<16> a)
442 {
443 fixed_point<16> r;
444 r.intValue = fixinv<16>(a.intValue);
445 return r;
446 }
447
448 } // end namespace fp
449
450 #endif
451
452