import numpy as np

def quat2rot(q):
    r, x, y, z = q[0], q[1], q[2], q[3]
    return np.array([
        [1. - 2. * (y * y + z * z), 2. * (x * y - r * z), 2. * (x * z + r * y)],
		[2. * (x * y + r * z), 1. - 2. * (x * x + z * z), 2. * (y * z - r * x)],
		[2. * (x * z - r * y), 2. * (y * z + r * x), 1. - 2. * (x * x + y * y)]
    ], dtype=np.float32)

def rot2quat(r):
    if r[2][2] < 0:
        if r[0][0] > r[1][1]:
            t = 1 + r[0][0] - r[1][1] - r[2][2]
            q = np.array([t, r[0][1] + r[1][0], r[0][2] + r[2][0], r[1][2] - r[2][1]])
        else:
            t = 1 - r[0][0] + r[1][1] - r[2][2]
            q = np.array([r[0][1] + r[1][0], t, r[1][2] + r[2][1], r[2][0] - r[0][2]])
    else:
        if r[0][0] < -r[1][1]:
            t = 1 - r[0][0] - r[1][1] + r[2][2]
            q = np.array([r[2][0] + r[0][2], r[1][2] + r[2][1], t, r[0][1] - r[1][0]])
        else:
            t = 1 + r[0][0] + r[1][1] + r[2][2]
            q = np.array([r[1][2] - r[2][1], r[2][0] + r[0][2], r[0][1] - r[1][0], t])
    return q * 0.5 / np.sqrt(t)

def sq2cov(s, q):
    S = np.diag(s)
    R = quat2rot(q)
    M = S @ R
    return M.T @ M

def mat_log(m):
    eigenvalues, eigenvectors = np.linalg.eig(m)
    return eigenvectors @ np.diag(np.log(eigenvalues)) @ np.linalg.inv(eigenvectors)