使用c语言将旋转矩阵转换为四元数

将旋转矩阵转换为四元数的公式如下：

$w = \sqrt{1 + m_{11} + m_{22} + m_{33}}/2$

$x = (m_{32} - m_{23}) / (4*w)$

$y = (m_{13} - m_{31}) / (4*w)$

$z = (m_{21} - m_{12}) / (4*w)$

其中，$w, x, y, z$ 分别代表四元数的实部和虚部，$m_{ij}$ 代表旋转矩阵的第 $i$ 行第 $j$ 列的元素。

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

#include <math.h>

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

void matrixToQuaternion(float m[3][3], Quaternion* q) {
    float w, x, y, z;
    float trace = m[0][0] + m[1][1] + m[2][2];

    if (trace > 0) {
        float s = sqrt(trace + 1.0);
        w = s / 2.0;
        s = 0.5 / s;
        x = (m[1][2] - m[2][1]) * s;
        y = (m[2][0] - m[0][2]) * s;
        z = (m[0][1] - m[1][0]) * s;
    } else {
        int i = 0;
        if (m[1][1] > m[0][0]) i = 1;
        if (m[2][2] > m[i][i]) i = 2;
        int j = (i + 1) % 3;
        int k = (j + 1) % 3;

        float s = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
        float q[4] = {0};
        q[i] = s / 2.0;
        s = 0.5 / s;
        q[3] = (m[j][k] - m[k][j]) * s;
        q[j] = (m[i][j] + m[j][i]) * s;
        q[k] = (m[i][k] + m[k][i]) * s;
        w = q[3];
        x = q[0];
        y = q[1];
        z = q[2];
    }

    q->w = w;
    q->x = x;
    q->y = y;
    q->z = z;
}

其中，`float m[3][3]` 代表旋转矩阵，`Quaternion* q` 代表四元数的指针。函数会将计算得到的四元数保存在传入的 `q` 指针所指向的内存中。