VR-四元素、旋转矩阵、欧拉角转换

public class RotateUtils {
    /**
     * 四元素转欧拉角（-180°, 180°）
     *
     * @param q
     * @return
     */
    public static Vector3f toEuler(Quaternionf q) {
        float s = 0;
        float sinHalfAngle = (float) Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
        if (sinHalfAngle > 0) {
            float cosHalfAngle = q.w;
            float halfAngle = (float) Math.atan2(sinHalfAngle, cosHalfAngle);

            // Ensure minimum rotation magnitude
            if (cosHalfAngle < 0)
                halfAngle -= Math.PI;

            s = 2 * halfAngle / sinHalfAngle;
        }
        return new Vector3f(q.x * s, q.y * s, q.z * s);
    }

    /**
     * 欧拉角转四元素
     *
     * @param euler
     * @return
     */
    public static Quaternionf toQuat(Vector3f euler) {
        float cosx = (float) Math.cos(euler.x / 2.0f);
        float cosy = (float) Math.cos(euler.y / 2.0f);
        float cosz = (float) Math.cos(euler.z / 2.0f);
        float sinx = (float) Math.sin(euler.x / 2.0f);
        float siny = (float) Math.sin(euler.y / 2.0f);
        float sinz = (float) Math.sin(euler.z / 2.0f);
        Quaternionf quat = new Quaternionf();
        quat.w = cosz * cosy * cosx - sinz * siny * sinx;
        quat.x = cosz * cosy * sinx + sinz * siny * cosx;
        quat.y = cosz * siny * cosx - sinz * cosy * sinx;
        quat.z = sinz * cosy * cosx + cosz * siny * sinx;
        quat.normalize();
        return quat;
    }

    /**
     * 旋转矩阵转欧拉角
     *
     * @param R
     * @return
     */
    public static Vector3f toEuler(Matrix4f R) {
        float sy = (float) Math.sqrt(R.m00() * R.m00() + R.m10() * R.m10());
        boolean singular = sy < 1e-6; // If
        float x, y, z;
        if (!singular) {
            x = (float) Math.atan2(R.m21(), R.m22());
            y = (float) Math.atan2(-R.m20(), sy);
            z = (float) Math.atan2(R.m10(), R.m00());
        } else {
            x = (float) Math.atan2(-R.m12(), R.m11());
            y = (float) Math.atan2(-R.m20(), sy);
            z = 0;
        }
        return new Vector3f(x, y, z);
    }

    /**
     * 旋转矩阵转四元素
     *
     * @param m
     * @return
     */
    public static Quaternionf toQuat(Matrix4f m) {
        Quaternionf quat = new Quaternionf();
        float[] tp = new float[4];
        tp[0] = 1 + m.m00() + m.m11() + m.m22();
        tp[1] = 1 + m.m00() - m.m11() - m.m22();
        tp[2] = 1 - m.m00() + m.m11() - m.m22();
        tp[3] = 1 - m.m00() - m.m11() + m.m22();

        int j = 0;
        for (int i = 1; i < 4; i++) {
            j = tp[i] > tp[i] ? i : j;
        }

        float qx, qy, qz, qw;
        if (j == 0) {
            qw = tp[0];
            qx = m.m12() - m.m21();
            qy = m.m20() - m.m02();
            qz = m.m01() - m.m10();
        } else if (j == 1) {
            qw = m.m12() - m.m21();
            qx = tp[1];
            qy = m.m01() + m.m10();
            qz = m.m20() + m.m02();
        } else if (j == 2) {
            qw = m.m20() - m.m02();
            qx = m.m01() + m.m10();
            qy = tp[2];
            qz = m.m12() + m.m21();
        } else {
            qw = m.m01() - m.m10();
            qx = m.m20() + m.m02();
            qy = m.m12() + m.m21();
            qz = tp[3];
        }
        float s = (float) Math.sqrt(0.25f * tp[j]);
        quat.set(qx * s, qy * s, qz * s, qw * s);
        quat.normalize();
        return quat;
    }

    /**
     * 四元素转旋转矩阵
     *
     * @param quat
     * @return
     */
    public static Matrix4f toMatrix(Quaternionf quat) {
        Matrix4f m = new Matrix4f();
        m.assumeNothing();
        return m.set(quat);
    }

    private static Vector3f mControllerEulerOffset = new Vector3f();

  /**
     * 重置YAW方向
     */
    public static Quaternionf recenter(Quaternionf dest, float[] src, boolean isRecentered) {
        if (src.length == 4 && dest != null) {
            Quaternionf controllerQuat = new Quaternionf(src[0], src[1], src[2], src[3]);
            if (isRecentered) {
                Vector3f euler = RotateUtils.toEuler(controllerQuat);
                mControllerEulerOffset.set(0, -euler.y, 0);
            }
            dest.set(RotateUtils.toQuat(mControllerEulerOffset).mul(controllerQuat));
        }
        return dest;
    }
}