C语言实现矩阵转四元数

要将一个矩阵转换为四元数，需要使用数学上的矩阵转换公式。下面是一个实现矩阵转四元数的 C 语言代码示例：

#include <math.h>

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

Quaternion matrixToQuaternion(double m[3][3]) {
    Quaternion q;
    double trace = m[0][0] + m[1][1] + m[2][2];

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

    return q;
}

这个函数接受一个 3x3 的矩阵作为输入，返回一个四元数。在函数内部，首先计算矩阵的轨迹（trace），然后根据轨迹的值选择不同的计算方式，最终得到四元数。