优化版 InvSqrt 算法:快速求逆平方根

需积分: 34 2 下载量 34 浏览量 更新于2024-10-07 收藏 148KB PDF 举报
" InvSqrt程序算法是一种快速计算平方根倒数的方法,源自于游戏开发领域,用于在牺牲部分精度的情况下大幅提高计算速度。该算法由Chris Lomont改进并分析,常见于在线代码库中。算法的核心在于一个特定的二进制常量0x5f3759df,通过位操作快速获取初始近似值,然后利用牛顿迭代法提高精度。" 在计算机科学中,计算平方根的逆元(即1/√x)是一个常见的数学运算,特别是在图形学、物理模拟和科学计算等领域。传统的平方根倒数计算通常依赖于浮点数的除法和平方根函数,这些操作在硬件层面执行时速度较慢。InvSqrt算法则提供了一种快速但略带误差的替代方案。 算法的主体分为以下几个步骤: 1. 将输入的浮点数x乘以0.5f,得到x的一半xhalf。 2. 通过位转换将浮点数x转换为整数i。这是因为浮点数在计算机内部是以二进制浮点格式存储的,包含指数和尾数两部分。这个步骤可以获取x的二进制表示。 3. 应用神秘的二进制常量0x5f3759df,并对整数i进行位移操作,得到初始的近似值y0。这个常量是一个经过精心选择的值,可以为后续迭代提供一个相对接近实际结果的初始估计。 4. 再次进行位转换,将整数i转换回浮点数形式,得到x。 5. 使用牛顿迭代法逐步改进近似值。迭代公式为x = x * (1.5f - xhalf * x * x)。这个步骤会反复执行,每次迭代都会减小与真实值的误差,直到达到所需的精度。 原始代码中的牛顿迭代通常只需要一次迭代就能达到相当高的精度,但对于要求更高精度的应用,可以增加迭代次数。值得注意的是,尽管InvSqrt算法速度快,但它可能会导致一定的精度损失。对于大多数实时渲染和游戏应用,这种损失是可以接受的,因为它显著提升了性能。 InvSqrt算法是计算机科学中一个巧妙的优化实例,它通过位操作和迭代优化,实现了比标准库函数更快的平方根倒数计算。这种方法尤其适用于对实时性能有严格要求的场景,例如视频游戏和高性能计算。然而,在需要极高精度的场合,可能还是需要依赖传统的浮点运算方法。

/* @brief @param[in] gx gy gz 为各轴角速度,单位为rad/s @param[in] ax ay az 为各轴加速度,单位为m/s^2 @param[in] halfT 为更新周期的一半,单位为s @param[out] pitch roll yaw 为当前欧拉角,单位为度 */ float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float q0temp, q1temp, q2temp, q3temp; float vx, vy, vz; float ex, ey, ez; float ix = 0, iy = 0, iz = 0; float kp = 1, ki = 0; void func(float *pitch, float *roll, float *yaw, float gx, float gy, float gz, float ax, float ay, float az, float halfT) { float norm; if(ax * ay *az != 0) { /* 归一化加速度 */ norm = inVSqrt(ax*ax + ay*ay + az*az); ax = ax * norm; ay = ay * norm; az = az * norm; /* 计算当前各轴加速度 */ vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; /* 计算加速度正交 */ ex = (ay*vz - az*vy) ; ey = (az*vx - ax*vz) ; ez = (ax*vy - ay*vx) ; /* 融合 */ ix += ex; iy += ey; iz += ez; gx = gx + kp*ex + ki*ix; gy = gy + kp*ey + ki*iy; gz = gz + kp*ez + ki*iz; } q0temp=q0; q1temp=q1; q2temp=q2; q3temp=q3; q0 = q0temp + (-q1temp*gx - q2temp*gy - q3temp*gz)*halfT; q1 = q1temp + ( q0temp*gx + q2temp*gz - q3temp*gy)*halfT; q2 = q2temp + ( q0temp*gy - q1temp*gz + q3temp*gx)*halfT; q3 = q3temp + ( q0temp*gz + q1temp*gy - q2temp*gx)*halfT; norm = inVSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 * norm; q1 = q1 * norm; q2 = q2 * norm; q3 = q3 * norm; *roll = atan2(2 * (q2 * q3 + q0 * q1), q0*q0 - q1*q1 - q2*q2 + q3*q3)* 57.295773f; *pitch = -asin(2 * (q1 * q3 - q0 * q2))*57.295773f; *yaw = atan2(2 * (q1 * q2 - q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3)*57.295773f; }

2023-06-10 上传