Crazepony开源四轴飞行器

Carmack平方根函数经典案例

作者: nieyong

编者注:在Crazepony飞控代码姿态解算融合中,有一个只有短短不到十行的函数,求解平方根的倒数。该函数是著名的Carmack平方根求解算法,拥有极高的运算效率,堪称经典。本文就介绍该函数背后的故事,由Craze团队-nieyong整理编辑。

首先粘贴Crazepony飞控代码IMUSO3.c文件中的该函数,求一个浮点数的平方根的倒数。

static float invSqrt(float number) 
{
    volatile long i;
    volatile float x, y;
    volatile const float f = 1.5F;

    x = number * 0.5F;
    y = number;
    i = * (( long * ) &y);
    i = 0x5f375a86 - ( i >> 1 );
    y = * (( float * ) &i);
    y = y * ( f - ( x * y * y ) );
    return y;
}

Quake-III Arena(雷神之锤3)是90年代的经典游戏之一。该系列的游戏不但画面和内容不错,而且即使计算机配置低,也能极其流畅地运行。这要归功于它3D引擎的开发者约翰-卡马克(John Carmack)。事实上早在90年代初DOS时代,只要能在PC上搞个小动画都能让人惊叹一番的时候,John Carmack就推出了石破天惊的Castle Wolfstein, 然后再接再励,doom, doom-II, Quake...每次都把3D技术推到极致。他的3D引擎代码极度高效,几乎是在压榨PC机的每条运算指令。当初微软的Direct3D也得听取他的意见,修改了不少API。

最近,Quake的开发商ID SOFTWARE遵守GPL协议,公开了Quake-III的原代码,让世人有幸目睹Carmack传奇的3D引擎的原码。

我们知道,越底层的函数,调用越频繁。3D引擎归根到底还是数学运算。那么找到最底层的数学运算函数(在game/code/q_math.c),必然是精心编写的。里面有很多有趣的函数,很多都令人惊奇。在game/code/q_math.c里发现了这样一段代码。它的作用是将一个数开平方并取倒,经测试这段代码比(float)(1.0/sqrt(x))快4倍。

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 fuck?
    y = * ( float * ) &i;
    y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration
    // y   = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed

    #ifndef Q3_VM
    #ifdef __linux__
         assert( !isnan(y) ); // bk010122 - FPE?
    #endif
    #endif
    return y;
}

函数返回平方根的倒数,即1/sqrt(x),这个函数在图像处理中比标准函数sqrt(x)更有用。

注意到这个函数只用了一次叠代。编译,实验,这个函数不仅工作的很好,而且比标准的sqrt()函数快4倍!要知道,编译器自带的函数,可是经过严格仔细的汇编优化的啊!这个简洁的函数,最核心,也是最让人费解的,就是标注了“what the fuck?”的一句

i = 0x5f3759df - ( i >> 1 );

再加上y = y * ( threehalfs - ( x2 * y * y ) );,两句话就完成了开方运算!而且注意到,核心那句是定点移位运算,速度极快!特别在很多没有乘法指令的RISC结构CPU上,这样做是极其高效的。

算法的原理其实不复杂,就是牛顿迭代法,用x-f(x)/f'(x)来不断的逼近f(x)=a的根。

一般的求平方根都是这么循环迭代算的,但是卡马克真正牛B的地方是他选择了一个神秘的常数0x5f3759df来计算那个猜测值,就是加注释的那一行,那一行算出的值非常接近1/sqrt(n),这样我们只需要2次牛顿迭代就可以达到我们所需要的精度。好吧如果这个还不算厉害,接着看。

普渡大学的数学家Chris Lomont看了以后觉得有趣,决定要研究一下卡马克弄出来的这个猜测值有什么奥秘。Lomont也是个牛人,在精心研究之后从理论上也推导出一个最佳猜测值,和卡马克的数字非常接近, 0x5f37642f。Lomont计算出结果以后非常满意,于是拿自己计算出的最佳猜测值和卡马克的神秘数字做比赛,看看谁的数字能够更快更精确的求得平方根。结果是卡马克赢了...谁也不知道卡马克是怎么找到这个数字的。

最后Lomont怒了,采用暴力方法一个数字一个数字试过来,终于找到一个比卡马克数字要好上那么一丁点的数字,虽然实际上这两个数字所产生的结果非常近似,这个暴力得出的数字是0x5f375a86。Crazepony飞控代码中,就是使用的这个值。

Lomont为此写下一篇论文,"Fast Inverse Square Root"。论文下载地址:http://www.math.purdue.edu/~clomont/Math/Papers/2003/InvSqrt.pdf或者http://www.matrix67.com/data/InvSqrt.pdf。参考

大家可以尝试在PC机、51、AVR、430、ARM、上面编译并实验,惊讶一下它的工作效率。

有则新闻,大意是说Ryszard Sommefeldt很久以前看到这段code,他一看之下惊为天人,想要拜见这位前辈高人,但是一路追寻下去却一直找不到人。在大家追寻的过程中,有人提到一份叫做MIT HACKMEM的文件,这是1970年代的MIT强者们做的一些笔记 (hack memo),大部份是algorithm,有些 code 是PDP-10 asm写的,另外有少数是C code (有人整理了一份列表)。

这个函数之所以重要,是因为求平方根倒数这个动作在3D运算(向量运算的部份)里面常常会用到,如果你用最原始的sqrt()然后再倒数的话,速度比上面的这个版本大概慢了四倍吧。当然,在飞控多旋翼的姿态解算中,同样涉及到很多向量运算,需要求平方根倒数,所以我们Crazepony选用了这样一个经典的函数。数学,算法是软件的根基,无论是对于游戏3D引擎还是无人机多旋翼飞控,都是相通的。Carmack平方根函数就是很好的证明。