def rotation_matrix_to_quaternion(rotation_matrix):
invert = (np.linalg.det(rotation_matrix) < <NUM_LIT:0>)<EOL>if invert:<EOL><INDENT>factor = -<NUM_LIT:1><EOL><DEDENT>else:<EOL><INDENT>factor = <NUM_LIT:1><EOL><DEDENT>c2 = <NUM_LIT>*(factor*np.trace(rotation_matrix) + <NUM_LIT:1>)<EOL>if c2 < <NUM_LIT:0>:<EOL><INDENT>c2 = <NUM_LIT:0.0><EOL><DEDENT>c = np.sqrt(c2)<EOL>r2 = <NUM_LIT:0.5>*(<NUM_LIT:1> + factor*np.diagonal(rotation_matrix)) - c2<EOL>r = np.zeros(<NUM_LIT:3>, float)<EOL>for index, r2_comp in enumerate(r2):<EOL><INDENT>if r2_comp < <NUM_LIT:0>:<EOL><INDENT>continue<EOL><DEDENT>else:<EOL><INDENT>row, col = off_diagonals[index]<EOL>if (rotation_matrix[row, col] - rotation_matrix[col, row] < <NUM_LIT:0>):<EOL><INDENT>r[index] = -np.sqrt(r2_comp)<EOL><DEDENT>else:<EOL><INDENT>r[index] = +np.sqrt(r2_comp)<EOL><DEDENT><DEDENT><DEDENT>return factor, np.array([c, r[<NUM_LIT:0>], r[<NUM_LIT:1>], r[<NUM_LIT:2>]], float)<EOL>
Compute the quaternion representing the rotation given by the matrix