blob: 13d31a8b5ed748ad7652857c1db8dc430eec4a6a (
plain) (
blame)
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
|
#include "fix16.h"
/* The square root algorithm is quite directly from
* http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Binary_numeral_system_.28base_2.29
* An important difference is that it is split to two parts
* in order to use only 32-bit operations.
*
* Note that for negative numbers we return -sqrt(-inValue).
* Not sure if someone relies on this behaviour, but not going
* to break it for now. It doesn't slow the code much overall.
*/
fix16_t fix16_sqrt(fix16_t inValue) {
uint8_t neg = (inValue < 0);
uint32_t num = (neg ? -inValue : inValue);
uint32_t result = 0;
uint32_t bit;
uint8_t n;
// Many numbers will be less than 15, so
// this gives a good balance between time spent
// in if vs. time spent in the while loop
// when searching for the starting value.
if (num & 0xFFF00000)
bit = (uint32_t)1 << 30;
else
bit = (uint32_t)1 << 18;
while (bit > num) bit >>= 2;
// The main part is executed twice, in order to avoid
// using 64 bit values in computations.
for (n = 0; n < 2; n++)
{
// First we get the top 24 bits of the answer.
while (bit)
{
if (num >= result + bit)
{
num -= result + bit;
result = (result >> 1) + bit;
}
else
{
result = (result >> 1);
}
bit >>= 2;
}
if (n == 0)
{
// Then process it again to get the lowest 8 bits.
if (num > 65535)
{
// The remainder 'num' is too large to be shifted left
// by 16, so we have to add 1 to result manually and
// adjust 'num' accordingly.
// num = a - (result + 0.5)^2
// = num + result^2 - (result + 0.5)^2
// = num - result - 0.5
num -= result;
num = (num << 16) - 0x8000;
result = (result << 16) + 0x8000;
}
else
{
num <<= 16;
result <<= 16;
}
bit = 1 << 14;
}
}
#ifndef FIXMATH_NO_ROUNDING
// Finally, if next bit would have been 1, round the result upwards.
if (num > result)
{
result++;
}
#endif
return (neg ? -result : result);
}
|
