decomposetrs.cpp

void decomposeMatrix(const float* matrix, float translation[3], float rotation[4], float scale[3])
{
	translation[0] = matrix[12];
	translation[1] = matrix[13];
	translation[2] = matrix[14];

	// 0 1 2
	// 4 5 6
	// 8 9 10
	float det =
	    matrix[0] * (matrix[5] * matrix[10] - matrix[6] * matrix[9]) -
	    matrix[1] * (matrix[4] * matrix[10] - matrix[6] * matrix[8]) +
	    matrix[2] * (matrix[4] * matrix[9] - matrix[5] * matrix[8]);

	scale[0] = (det < 0 ? -1 : +1) * sqrtf(matrix[0] * matrix[0] + matrix[1] * matrix[1] + matrix[2] * matrix[2]);
	scale[1] = sqrtf(matrix[4] * matrix[4] + matrix[5] * matrix[5] + matrix[6] * matrix[6]);
	scale[2] = sqrtf(matrix[8] * matrix[8] + matrix[9] * matrix[9] + matrix[10] * matrix[10]);

	float qx = 0, qy = 0, qz = 0, qw = 1;

	if (det != 0.f)
	{
		float m00 = matrix[0] / scale[0];
		float m01 = matrix[1] / scale[0];
		float m02 = matrix[2] / scale[0];

		float m10 = matrix[4] / scale[1];
		float m11 = matrix[5] / scale[1];
		float m12 = matrix[6] / scale[1];

		float m20 = matrix[8] / scale[2];
		float m21 = matrix[9] / scale[2];
		float m22 = matrix[10] / scale[2];

		if (m00 + m11 + m22 > 0)
		{
			qx = m21 - m12;
			qy = m02 - m20;
			qz = m10 - m01;
			qw = 1 + m00 + m11 + m22;
		}
		else if (m00 > m11 && m00 > m22)
		{
			qx = 1 + m00 - m11 - m22;
			qy = m01 + m10;
			qz = m02 + m20;
			qw = m21 - m12;
		}
		else if (m11 > m22)
		{
			qx = m01 + m10;
			qy = 1 + m11 - m00 - m22;
			qz = m12 + m21;
			qw = m02 - m20;
		}
		else
		{
			qx = m02 + m20;
			qy = m12 + m21;
			qz = 1 + m22 - m00 - m11;
			qw = m10 - m01;
		}

		float ql = sqrtf(qx * qx + qy * qy + qz * qz + qw * qw);

		rotation[0] = qx / ql;
		rotation[1] = qy / ql;
		rotation[2] = qz / ql;
		rotation[3] = qw / ql;
	}
}