旋转矩阵转坐标四元数C代码实现，并且解释逻辑

下面是旋转矩阵转坐标四元数的 C 代码实现：

#include <math.h>

typedef struct {
    double w, x, y, z;
} Quaternion;

Quaternion from_rotation_matrix(double m[3][3]) {
    double t = m[0][0] + m[1][1] + m[2][2];
    double qw, qx, qy, qz;

    if (t > 0) {
        double r = sqrt(1 + t);
        qw = 0.5 * r;
        qx = (m[2][1] - m[1][2]) / (2 * r);
        qy = (m[0][2] - m[2][0]) / (2 * r);
        qz = (m[1][0] - m[0][1]) / (2 * r);
    } else if ((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])) {
        double r = sqrt(1 + m[0][0] - m[1][1] - m[2][2]);
        qw = (m[2][1] - m[1][2]) / (2 * r);
        qx = 0.5 * r;
        qy = (m[0][1] + m[1][0]) / (2 * r);
        qz = (m[0][2] + m[2][0]) / (2 * r);
    } else if (m[1][1] > m[2][2]) {
        double r = sqrt(1 - m[0][0] + m[1][1] - m[2][2]);
        qw = (m[0][2] - m[2][0]) / (2 * r);
        qx = (m[0][1] + m[1][0]) / (2 * r);
        qy = 0.5 * r;
        qz = (m[1][2] + m[2][1]) / (2 * r);
    } else {
        double r = sqrt(1 - m[0][0] - m[1][1] + m[2][2]);
        qw = (m[1][0] - m[0][1]) / (2 * r);
        qx = (m[0][2] + m[2][0]) / (2 * r);
        qy = (m[1][2] + m[2][1]) / (2 * r);
        qz = 0.5 * r;
    }

    Quaternion q = {qw, qx, qy, qz};
    return q;
}

该代码实现了从一个旋转矩阵到坐标四元数的转换。坐标四元数是一个四元组 $(w, x, y, z)$，其中 $w$ 为实数，$x, y, z$ 为三个虚数。与矩阵不同，坐标四元数可以扩展到四维空间，能够精确地表示旋转中的所有变换。

下面解释一下代码的逻辑。这个函数接受一个 $3 \times 3$ 的旋转矩阵 $m$，返回对应的坐标四元数 $q$。

我们可以利用勾股定理和三角函数来计算旋转矩阵中的角度和方向，然后把它们转化为坐标四元数。首先，我们计算旋转矩阵中所有元素的和 $t = m_{00} + m_{11} + m_{22}$。如果 $t$ 是正数，那么我们可以使用递推公式计算出 $q$ 的分量：

$$\begin{aligned}
w &= \frac{1}{2} \sqrt{1 + t} \\
x &= \frac{m_{21} - m_{12}}{2w} \\
y &= \frac{m_{02} - m_{20}}{2w} \\
z &= \frac{m_{10} - m_{01}}{2w} \\
\end{aligned}$$

否则，我们需要考虑 $m$ 中对角线元素的关系。如果 $m_{00}$ 最大，那么我们可以计算出：

$$\begin{aligned}
x &= \frac{1}{2} \sqrt{1 + m_{00} - m_{11} - m_{22}} \\
y &= \frac{m_{01} + m_{10}}{2x} \\
z &= \frac{m_{02} + m_{20}}{2x} \\
w &= \frac{m_{21} - m_{12}}{2x} \\
\end{aligned}$$

如果 $m_{11}$ 最大，那么我们可以计算出：

$$\begin{aligned}
y &= \frac{1}{2} \sqrt{1 - m_{00} + m_{11} - m_{22}} \\
x &= \frac{m_{01} + m_{10}}{2y} \\
z &= \frac{m_{12} + m_{21}}{2y} \\
w &= \frac{m_{02} - m_{20}}{2y} \\
\end{aligned}$$

如果 $m_{22}$ 最大，那么我们可以计算出：

$$\begin{aligned}
z &= \frac{1}{2} \sqrt{1 - m_{00} - m_{11} + m_{22}} \\
x &= \frac{m_{02} + m_{20}}{2z} \\
y &= \frac{m_{12} + m_{21}}{2z} \\
w &= \frac{m_{10} - m_{01}}{2z} \\
\end{aligned}$$

最后，如果开始的求和 $t$ 是负数，那么我们可以计算出：

$$\begin{aligned}
w &= \frac{m_{21} - m_{12}}{2r} \\
x &= \frac{m_{02} + m_{20}}{2r} \\
y &= \frac{m_{10} + m_{01}}{2r} \\
z &= \frac{m_{12} + m_{21}}{2r} \\
\end{aligned}$$

其中 $r = \sqrt{1 - t}$。