VR开发-罗德里格公式的研究

最近在做VR方面的研究，看到了Google Cardboard对传感器处理的代码，对旋转矩阵、罗德里格公式做了一些研究。
罗德里格公式是计算三维空间中，一个向量绕旋转轴旋转给定角度以后得到的新向量的计算公式，具体的详细推导可以看维基，我觉得只要能知道如何使用就可以了，注意旋转轴为单位向量，来计算旋转矩阵。
https://en.wikipedia.org/wiki/Rotation_matrix
http://mathworld.wolfram.com/RodriguesRotationFormula.html

下面是Google Cardboard中罗德里格公式代码：

    public static void sO3FromMu(Vector3d w, Matrix3x3d result) {
        double kA;
        double kB;
        double thetaSq = Vector3d.dot(w, w);
        double theta = Math.sqrt(thetaSq);
        if (thetaSq < 1.0E-8) {
            kA = 1.0 - 0.1666666716337204 * thetaSq;
            kB = 0.5;
        } else if (thetaSq < 1.0E-6) {
            kB = 0.5 - 0.0416666679084301 * thetaSq;
            kA = 1.0 - thetaSq * 0.1666666716337204 * (1.0 - 0.1666666716337204 * thetaSq);
        } else {
            double invTheta = 1.0 / theta;
            kA = Math.sin(theta) * invTheta;
            kB = (1.0 - Math.cos(theta)) * (invTheta * invTheta);
        }
        So3Util.rodriguesSo3Exp(w, kA, kB, result);
    }
    private static void rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d result) {
        double wx2 = w.x * w.x;
        double wy2 = w.y * w.y;
        double wz2 = w.z * w.z;
        result.set(0, 0, 1.0 - kB * (wy2 + wz2));
        result.set(1, 1, 1.0 - kB * (wx2 + wz2));
        result.set(2, 2, 1.0 - kB * (wx2 + wy2));
        double a = kA * w.z;
        double b = kB * (w.x * w.y);
        result.set(0, 1, b - a);
        result.set(1, 0, b + a);
        a = kA * w.y;
        b = kB * (w.x * w.z);
        result.set(0, 2, b + a);
        result.set(2, 0, b - a);
        a = kA * w.x;
        b = kB * (w.y * w.z);
        result.set(1, 2, b - a);
        result.set(2, 1, b + a);
    }

代码中罗德里格变换矩阵为：

KA和KB两个参数在陀螺仪静态状态下，（x，y，z）角速度极小条件下做了修正，效果应该是能够增强画面移动的稳定性，这个还有待测试。
这里为了计算方便有个推导公式：

这样就和上面公式一样了。