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）角速度极小条件下做了修正，效果应该是能够增强画面移动的稳定性，这个还有待测试。
这里为了计算方便有个推导公式：

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