diff --git a/dda_maths.c b/dda_maths.c index 4952b1a..54bc8fa 100644 --- a/dda_maths.c +++ b/dda_maths.c @@ -178,6 +178,44 @@ uint16_t int_sqrt(uint32_t a) { return x; } +/*! + integer inverse square root algorithm + \param a find the inverse of the square root of this number + \return 0x1000 / sqrt(a) - 1 < returnvalue <= 0x1000 / sqrt(a) + + This is a binary search but it uses only the minimum required bits for each step. +*/ +uint16_t int_inv_sqrt(uint16_t a) { + /// 16bits inverse (much faster than doing a full 32bits inverse) + /// the 0xFFFFU instead of 0x10000UL hack allows using 16bits and 8bits + /// variable for the first 8 steps without overflowing and it seems to + /// give better results for the ramping equation too :) + uint8_t z = 0, i; + uint16_t x, j; + uint32_t q = ((uint32_t)(0xFFFFU / a)) << 8; + + for (i = 0x80; i; i >>= 1) { + uint16_t y; + + z |= i; + y = (uint16_t)z * z; + if (y > (q >> 8)) + z ^= i; + } + + x = z << 4; + for (j = 0x8; j; j >>= 1) { + uint32_t y; + + x |= j; + y = (uint32_t)x * x; + if (y > q) + x ^= j; + } + + return x; +} + // this is an ultra-crude pseudo-logarithm routine, such that: // 2 ^ msbloc(v) >= v /*! crude logarithm algorithm diff --git a/dda_maths.h b/dda_maths.h index c51fd47..7b09b74 100644 --- a/dda_maths.h +++ b/dda_maths.h @@ -59,6 +59,9 @@ uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz); // integer square root algorithm uint16_t int_sqrt(uint32_t a); +// integer inverse square root, 12bits precision +uint16_t int_inv_sqrt(uint16_t a); + // this is an ultra-crude pseudo-logarithm routine, such that: // 2 ^ msbloc(v) >= v const uint8_t msbloc (uint32_t v);