通过旋转矩阵求四元数的c语言代码

以下是通过旋转矩阵求四元数的C语言代码：

#include <stdio.h>
#include <math.h>

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

Quaternion fromRotationMatrix(double matrix[3][3]) {
    Quaternion q;
    double trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
    if (trace > 0) {
        double s = 0.5 / sqrt(trace + 1.0);
        q.w = 0.25 / s;
        q.x = (matrix[2][1] - matrix[1][2]) * s;
        q.y = (matrix[0][2] - matrix[2][0]) * s;
        q.z = (matrix[1][0] - matrix[0][1]) * s;
    } else {
        if (matrix[0][0] > matrix[1][1] && matrix[0][0] > matrix[2][2]) {
            double s = 2.0 * sqrt(1.0 + matrix[0][0] - matrix[1][1] - matrix[2][2]);
            q.w = (matrix[2][1] - matrix[1][2]) / s;
            q.x = 0.25 * s;
            q.y = (matrix[0][1] + matrix[1][0]) / s;
            q.z = (matrix[0][2] + matrix[2][0]) / s;
        } else if (matrix[1][1] > matrix[2][2]) {
            double s = 2.0 * sqrt(1.0 + matrix[1][1] - matrix[0][0] - matrix[2][2]);
            q.w = (matrix[0][2] - matrix[2][0]) / s;
            q.x = (matrix[0][1] + matrix[1][0]) / s;
            q.y = 0.25 * s;
            q.z = (matrix[1][2] + matrix[2][1]) / s;
        } else {
            double s = 2.0 * sqrt(1.0 + matrix[2][2] - matrix[0][0] - matrix[1][1]);
            q.w = (matrix[1][0] - matrix[0][1]) / s;
            q.x = (matrix[0][2] + matrix[2][0]) / s;
            q.y = (matrix[1][2] + matrix[2][1]) / s;
            q.z = 0.25 * s;
        }
    }
    return q;
}

int main() {
    double matrix[3][3] = {
        {0.7071, -0.7071, 0},
        {0.7071, 0.7071, 0},
        {0, 0, 1}
    };
    Quaternion q = fromRotationMatrix(matrix);
    printf("Quaternion: w=%f, x=%f, y=%f, z=%f\n", q.w, q.x, q.y, q.z);
    return 0;
}

该程序定义了一个 `Quaternion` 结构体来表示四元数，其中 `w` 表示实部，`x`、`y` 和 `z` 表示虚部。`fromRotationMatrix` 函数接受一个 $3\times3$ 的旋转矩阵，返回对应的四元数。该函数的实现参考了《3D数学基础》一书中的算法。

在主函数中，我们构造了一个旋转矩阵，并通过 `fromRotationMatrix` 函数将其转换为四元数。最后将四元数的各个分量打印出来。