旋转矩阵转四元数C语言版本

void mat2qua(float(*m)[3], float* qua)
{
	float q1 = sqrt(m[0][0] + m[1][1] + m[2][2] + 1) / 2;
	float q2, q3, q4, tr, s;
	if (q1 != 0.0) {
		q2 = (m[2][1] - m[1][2]) / 4 / q1;
		q3 = (m[0][2] - m[2][0]) / 4 / q1;
		q4 = (m[1][0] - m[0][1]) / 4 / q1;
	}
	else {
		tr = m[0][0] + m[1][1] + m[2][2];
		if (tr > 0) {
			s = sqrt(tr + 1.0) * 2;
			q1 = 0.25 * s;
			q2 = (m[2][1] - m[1][2]) / s;
			q3 = (m[0][2] - m[2][0]) / s;
			q4 = (m[1][0] - m[0][1]) / s;
		}
		else if ((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])) {
			s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2;
			q1 = (m[2][1] - m[1][2]) / s;
			q2 = 0.25 * s;
			q3 = (m[0][1] + m[1][0]) / s;
			q4 = (m[0][2] + m[2][0]) / s;
		}
		else if(m[1][1] > m[2][2])
		{
			s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2;
			q1 = (m[0][2] - m[2][0]) / s;
			q2 = (m[0][1] + m[1][0]) / s;
			q3 = 0.25 * s;
			q4 = (m[1][2] + m[2][1]) / s;
		}
		else {
			s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2;
			q1 = (m[1][0] - m[0][1]) / s;
			q2 = (m[0][2] + m[2][0]) / s;
			q3 = (m[1][2] + m[2][1]) / s;
			q4 = 0.25 * s;
		}
	}
	qua[0] = q1;
	qua[1] = q2;
	qua[2] = q3;
	qua[3] = q4;
}