diff options
| author | Xavi Del Campo <xavi.dcr@tutanota.com> | 2020-01-31 10:32:23 +0100 |
|---|---|---|
| committer | Xavi Del Campo <xavi.dcr@tutanota.com> | 2020-01-31 10:32:23 +0100 |
| commit | 7c24e9a9b02b04dcaf9507acb94091ea70a2c02d (patch) | |
| tree | c28d0748652ad4b4222309e46e6cfc82c0906220 /libfixmath/fix16_sqrt.c | |
| parent | a2b7b6bb1cc2f4a3258b7b2dbc92399d151f864d (diff) | |
| download | psxsdk-7c24e9a9b02b04dcaf9507acb94091ea70a2c02d.tar.gz | |
Imported pristine psxsdk-20190410 from official repo
Diffstat (limited to 'libfixmath/fix16_sqrt.c')
| -rwxr-xr-x | libfixmath/fix16_sqrt.c | 83 |
1 files changed, 83 insertions, 0 deletions
diff --git a/libfixmath/fix16_sqrt.c b/libfixmath/fix16_sqrt.c new file mode 100755 index 0000000..13d31a8 --- /dev/null +++ b/libfixmath/fix16_sqrt.c @@ -0,0 +1,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);
+}
|