/* Copyright (C) 1986-1999 Ken Turkowski. <turk@computer.org>
 *
 * All rights reserved.
 *
 * Warranty Information
 *  Even though I have reviewed this software, I makes no warranty
 *  or representation, either express or implied, with respect to this
 *  software, its quality, accuracy, merchantability, or fitness for a
 *  particular purpose.  As a result, this software is provided "as is,"
 *  and you, its user, are assuming the entire risk as to its quality
 *  and accuracy.
 *
 * This code may be used and freely distributed as long as it includes
 *  this copyright notice and the above warranty information.
 *
 ********************************************************************************
 * InvSqrt calculates the inverse sqaure root of its argument.
 ********************************************************************************/

#include <stdlib.h>     /* for malloc() */
#include <math.h>


/* Prototype */
float InvSqrt(float x);


/********************************************************************************
 * InvSqrt() returns the inverse square root of its argument.
 ********************************************************************************/

/* Specified parameters */
#define LOOKUP_BITS    6   /* Number of mantissa bits for lookup */
#define EXP_POS       23   /* Position of the exponent */
#define EXP_BIAS     127   /* Bias of exponent */
/* The mantissa is assumed to be just down from the exponent */

/* Derived parameters */
#define LOOKUP_POS   (EXP_POS-LOOKUP_BITS)      /* Position of the mantissa lookup */
#define SEED_POS     (EXP_POS-8)                /* Position of the mantissa seed */
#define TABLE_SIZE   (2 << LOOKUP_BITS)         /* Number of entries in the table */
#define LOOKUP_MASK  (TABLE_SIZE - 1)           /* Mask for table input */
#define GET_EXP(a)   (((a) >> EXP_POS) & 0xFF)  /* Extract exponent */
#define SET_EXP(a)   ((a) << EXP_POS)           /* Set exponent */
#define GET_EMANT(a) (((a) >> LOOKUP_POS) & LOOKUP_MASK)    /* Extended mantissa MSB's */
#define SET_MANTSEED(a) (((unsigned int)(a)) << SEED_POS)   /* Set mantissa 8 MSB's */

static unsigned char *iSqrt = NULL;

union _flint {
    unsigned int    i;
    float            f;
};

static void MakeInverseSqrtLookupTable(void)
{
    register int f;
    register unsigned char *h;
    union _flint fi, fo;
    
    iSqrt = malloc(TABLE_SIZE);
    h = iSqrt;

    for (f = 0, h = iSqrt; f < TABLE_SIZE; f++) {
        fi.i = ((EXP_BIAS-1) << EXP_POS) | (f << LOOKUP_POS);
        fo.f = 1.0 / sqrt(fi.f);
        *h++ = ((fo.i + (1 << (SEED_POS-2))) >> SEED_POS) & 0xFF;  /* rounding */
    }
    iSqrt[TABLE_SIZE / 2] = 0xFF;    /* Special case for 1.0 */
}


float InvSqrt(float x)
{
    register unsigned int a = ((union _flint*)(&x))->i;
    register float arg = x;
    union _flint seed;
    register float r;
    
    if (iSqrt == NULL)
        MakeInverseSqrtLookupTable();
    
    seed.i = SET_EXP(((3*EXP_BIAS-1) - GET_EXP(a)) >> 1)
           | SET_MANTSEED(iSqrt[GET_EMANT(a)]);
    
    /* Seed: accurate to LOOKUP_BITS */
    r = seed.f;

    /* First iteration: accurate to 2*LOOKUP_BITS */
    r = (3.0 - r * r * arg) * r * 0.5;
    
    /* Second iteration: accurate to 4*LOOKUP_BITS */
    r = (3.0 - r * r * arg) * r * 0.5;
    
    return(r);
}