旋转矩阵转四元数C代码实现

以下是旋转矩阵转四元数的 C 代码实现，请参考：

#include <math.h>

void matrix_to_quaternion(float m[3][3], float q[4]) {
float trace = m[0][0] + m[1][1] + m[2][2];
if (trace > 0.0) {
float s = sqrt(trace + 1.0);
q[0] = s * 0.5;
s = 0.5 / s;
q[1] = (m[2][1] - m[1][2]) * s;
q[2] = (m[0][2] - m[2][0]) * s;
q[3] = (m[1][0] - m[0][1]) * s;
} else {
int i = m[0][0] < m[1][1] ?
(m[1][1] < m[2][2] ? 2 : 1) :
(m[0][0] < m[2][2] ? 2 : 0);
int j = (i + 1) % 3;
int k = (j + 1) % 3;
float s = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
q[i+1] = s * 0.5;
s = 0.5 / s;
q[0] = (m[k][j] - m[j][k]) * s;
q[j+1] = (m[j][i] + m[i][j]) * s;
q[k+1] = (m[k][i] + m[i][k]) * s;
}
}

感谢您的提问，请问还有什么问题我可以帮助回答？