Fast inverse of square root

Moderator: Benj

User avatar
Mantas
Posts: 221
Joined: Tue May 15, 2012 10:32 pm
Location: Klaipeda, Lithuania - North sea, UK
Has thanked: 68 times
Been thanked: 27 times
Contact:

Fast inverse of square root

Postby Mantas » Sun Dec 09, 2012 10:44 am

Hello MM,

As you know sometimes microcontrollers struggle with computation intensive functions, and you want to optimize the code to make life easier for them. So you probably remember the famous hack from Quake ||| to compute the inverse square root of x, if not, here is a remainder how it looks:

Code: Select all

float Q_rsqrt( float number )
{
        long i;
        float x2, y;
        const float threehalfs = 1.5F;
 
        x2 = number * 0.5F;
        y  = number;
        i  = * ( long * ) &y;                       // evil floating point bit level hacking
        i  = 0x5f3759df - ( i >> 1 );               // what the f***?
        y  = * ( float * ) &i;
        y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
//      y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed
 
        return y;
}


I was doing some really complex functions for quaternion calculations and decided to use the famous hack to improve on speed. But this function leads to max error of about 1.75233867·10−3 and average of 1.24792411·10−6.
I did a little bit of googling and found that one crazy kid found better coefficients for the same hack, and it looks like this:

Code: Select all

float inv_sqrt(float x)
{ union { float f; uint32 u; } y = {x};
  y.u = 0x5F1FFFF9ul - (y.u >> 1);
  return 0.703952253f * y.f * (2.38924456f - x * y.f * y.f);
}


This improves the error quite a lot leading to max of 6.50196699·10−4 and an average of 2.00010826·10−7. So my offer is to incorporate this cool function into FC library just in case someone needs it :)

Best regards,
Mantas

Edited out swear word as people under 18 could be reading forums..Martin
These users thanked the author Mantas for the post (total 4):
medelec35 (Sun Dec 09, 2012 11:39 am) • Enamul (Sun Dec 09, 2012 11:44 am) • JonnyW (Sun Dec 09, 2012 10:43 pm) • Benj (Mon Dec 10, 2012 11:41 am)
Rating: 21.05%
 
Science is my true religion.

User avatar
Mantas
Posts: 221
Joined: Tue May 15, 2012 10:32 pm
Location: Klaipeda, Lithuania - North sea, UK
Has thanked: 68 times
Been thanked: 27 times
Contact:

Re: Fast inverse of square root

Postby Mantas » Tue Dec 11, 2012 12:27 am

For the time being here is a macro if someone needs to use it in the FC before we have it in the library. I have attached a sample program if anyone needs to test it. You can clearly see how accurate the answer is.

Best regards,
Mantas
Attachments
invsqrt.fcf_arm
Sample program
(10 KiB) Downloaded 170 times
invSqrt.fcm
Fast inverse square root
(2.19 KiB) Downloaded 141 times
Science is my true religion.