c语言实现三维方向余弦矩阵转换为标准四元数

三维方向余弦矩阵（DCM）是一个 $3 \times 3$ 的旋转矩阵，可以表示物体在三维空间中的方向。如果要将 DCM 转换为标准四元数，可以按照以下步骤进行：

1. 计算矩阵的迹 $tr(\textbf{C})$，其中 $\textbf{C}$ 是 DCM 矩阵。
2. 计算旋转角度 $\theta$，$\theta = \cos^{-1}\left(\frac{tr(\textbf{C})-1}{2}\right)$。
3. 计算旋转轴 $\textbf{r}$，$\textbf{r} = \frac{1}{2\sin(\theta)}\begin{bmatrix}c_{32}-c_{23}\\c_{13}-c_{31}\\c_{21}-c_{12}\end{bmatrix}$，其中 $c_{ij}$ 表示 DCM 矩阵中第 $i$ 行、第 $j$ 列的元素。
4. 计算四元数 $q$，$q = [\cos(\theta/2), r_x \sin(\theta/2), r_y \sin(\theta/2), r_z \sin(\theta/2)]$，其中 $r_x, r_y, r_z$ 分别表示旋转轴 $\textbf{r}$ 在 $x, y, z$ 方向上的分量。

下面是 C 语言的实现代码：

#include <math.h>

typedef struct {
    float w;
    float x;
    float y;
    float z;
} Quaternion;

void dcm2quat(float C[3][3], Quaternion *q) {
    float tr = C[0][0] + C[1][1] + C[2][2];
    float theta = acos((tr - 1.0f) / 2.0f);
    float rx = (C[2][1] - C[1][2]) / (2.0f * sin(theta));
    float ry = (C[0][2] - C[2][0]) / (2.0f * sin(theta));
    float rz = (C[1][0] - C[0][1]) / (2.0f * sin(theta));
    q->w = cos(theta / 2.0f);
    q->x = rx * sin(theta / 2.0f);
    q->y = ry * sin(theta / 2.0f);
    q->z = rz * sin(theta / 2.0f);
}

其中，$C$ 是 $3 \times 3$ 的 DCM 矩阵，$q$ 是输出的四元数。