diff options
| -rw-r--r-- | apps/fixedpoint.c | 35 |
1 files changed, 21 insertions, 14 deletions
diff --git a/apps/fixedpoint.c b/apps/fixedpoint.c index fb89a8d..a8f606a 100644 --- a/apps/fixedpoint.c +++ b/apps/fixedpoint.c @@ -152,29 +152,36 @@ long fp_sincos(unsigned long phase, long *cos) * @return Square root of argument in same fixed point format as input. * * This routine has been modified to run longer for greater precision, - * but cuts calculation short if the answer is reached sooner. In - * general, the closer x is to 1, the quicker the calculation. + * but cuts calculation short if the answer is reached sooner. */ long fp_sqrt(long x, unsigned int fracbits) { - long b = x/2 + BIT_N(fracbits); /* initial approximation */ - long c; - unsigned n; - const unsigned iterations = 8; - - for (n = 0; n < iterations; ++n) + unsigned long xfp, b; + int n = 8; /* iteration limit (should terminate earlier) */ + + if (x <= 0) + return 0; /* no sqrt(neg), or just sqrt(0) = 0 */ + + /* Increase working precision by one bit */ + xfp = x << 1; + fracbits++; + + /* Get the midpoint between fracbits index and the highest bit index */ + b = ((sizeof(xfp)*8-1) - __builtin_clzl(xfp) + fracbits) >> 1; + b = BIT_N(b); + + do { - c = fp_div(x, b, fracbits); + unsigned long c = b; + b = (fp_div(xfp, b, fracbits) + b) >> 1; if (c == b) break; - b = (b + c)/2; } + while (n-- > 0); - return b; + return b >> 1; } -/* Accurate int sqrt with only elementary operations. (the above - * routine fails badly without enough iterations, more iterations - * than this requires -- [give that one a FIXME]). +/* Accurate int sqrt with only elementary operations. * Snagged from: * http://www.devmaster.net/articles/fixed-point-optimizations/ */ unsigned long isqrt(unsigned long x) |