使用c语言写一个很简介的旋转矩阵转四元数的函数，并将四元数进行单位化

以下是一个简单的 C 语言函数，用于将旋转矩阵转换为四元数并将其单位化：

#include <math.h>

void matrix_to_quaternion(float *m, float *q)
{
    float trace = m[0] + m[4] + m[8];
    if (trace > 0.0f) {
        float s = sqrtf(trace + 1.0f);
        q[3] = 0.5f * s;
        s = 0.5f / s;
        q[0] = (m[7] - m[5]) * s;
        q[1] = (m[2] - m[6]) * s;
        q[2] = (m[3] - m[1]) * s;
    } else {
        int i = 0;
        if (m[4] > m[0])
            i = 1;
        if (m[8] > m[i * 3 + i])
            i = 2;
        int j = (i + 1) % 3;
        int k = (j + 1) % 3;
        float s = sqrtf(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + 1.0f);
        q[i] = 0.5f * s;
        s = 0.5f / s;
        q[3] = (m[k * 3 + j] - m[j * 3 + k]) * s;
        q[j] = (m[j * 3 + i] + m[i * 3 + j]) * s;
        q[k] = (m[k * 3 + i] + m[i * 3 + k]) * s;
    }

    // Normalize quaternion
    float inv_len = 1.0f / sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    q[0] *= inv_len;
    q[1] *= inv_len;
    q[2] *= inv_len;
    q[3] *= inv_len;
}

该函数采用一个指向旋转矩阵的指针和一个指向四元数的指针作为输入参数，并将计算结果存储在四元数中。如果旋转矩阵是齐次坐标变换矩阵，则必须使用旋转部分（即左上角的3x3子矩阵）作为输入。

该函数首先计算矩阵的迹，以确定输入矩阵是否为正规的旋转矩阵。如果迹大于零，则矩阵是正规的，可以直接计算四元数。否则，它使用一个复杂的计算来确定旋转轴和旋转角度，并计算四元数。

最后，该函数通过将四元数除以其长度来规范化四元数。