import math

# Converts angle from degree to radian  

def to_radian(angle):

    return angle / 180.0 * math.pi

# Constructs a quaternion from a rotation of degree 'angle' around vector 'axis'

def quaternion(axis, angle):

    angle *= 0.5

    sinAngle = math.sin(to_radian(angle))

    return normalize((axis[0] * sinAngle, axis[1] * sinAngle, axis[2] * sinAngle, math.cos(to_radian(angle))))

# Normalizes quaternion 'q'

def normalize(q):

    length = math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3])

    return (q[0] / length, q[1] / length, q[2] / length, q[3] / length)

# Multiplies 2 quaternions : 'q1' * 'q2'    

def multiply_quat(q1, q2):

    return (q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],

            q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],

            q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],

            q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2])

# Converts quaternion 'q' to a rotation matrix

def matrix_from_quat(q):

    x2 = q[0] * q[0]

    y2 = q[1] * q[1]

    z2 = q[2] * q[2]

    xy = q[0] * q[1]

    xz = q[0] * q[2]

    yz = q[1] * q[2]

    wx = q[3] * q[0]

    wy = q[3] * q[1]

    wz = q[3] * q[2]

    return (1.0 - 2.0 * (y2 + z2), 2.0 * (xy - wz), 2.0 * (xz + wy), 0.0,

            2.0 * (xy + wz), 1.0 - 2.0 * (x2 + z2), 2.0 * (yz - wx), 0.0,

            2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (x2 + y2), 0.0,

            0.0, 0.0, 0.0, 1.0)

# Constructs a translation matrix

def matrix_from_trans(trans):

    return (1, 0, 0, trans[0],

            0, 1, 0, trans[1],

            0, 0, 1, trans[2],

            0, 0, 0, 1)

# Returns an identity matrix

def identity_matrix():

    return (1, 0, 0, 0,

            0, 1, 0, 0,

            0, 0, 1, 0,

            0, 0, 0, 1)

# Multiplies 2 Mat4 : 'm1' * 'm2'           

def multiply_matrix(m1, m2):

    res = []

    for i in range(0, 4):

        for j in range(0, 4):

            res.append(m1[i * 4] * m2[j] + m1[i * 4 + 1] * m2[j + 4] + m1[i * 4 + 2] * m2[j + 8] + m1[i * 4 + 3] * m2[j + 12])

    return res

# Multiplies matrix 'm' by vector 'v'

def multiply_mat_by_vec(m, v):

    w = 1.0

    if len(v) == 4:

        w = v[3]

    return (m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3] * w,

            m[4] * v[0] + m[5] * v[1] + m[6] * v[2] + m[7] * w,

            m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11] * w,

            m[12] * v[0] + m[13] * v[1] + m[14] * v[2] + m[15] * w)

# Transposes matrix 'm'

def transpose(m):

    return (m[0], m[4], m[8], m[12],

            m[1], m[5], m[9], m[13],

            m[2], m[6], m[10], m[14],

            m[3], m[7], m[11], m[15])

# Calculates inverse of matrix m

# Reimplement of gluInvertMatrix

def invert_matrix(m):

    inv = []

    inv.append(m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10])

    inv.append(-m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10])

    inv.append(m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6])

    inv.append(-m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6])

    inv.append(-m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10])

    inv.append(m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10])

    inv.append(-m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6])

    inv.append(m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6])

    inv.append(m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9])

    inv.append(-m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9])

    inv.append(m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5])

    inv.append(-m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5])

    inv.append(-m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9])

    inv.append(m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9])

    inv.append(-m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5])

    inv.append(m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5])

    det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]

    if det == 0:

        return None

    det = 1.0 / det

    mOut = []

    for i in range(16):

        mOut.append(inv[i] * det)

    return mOut

# Calculates length^2 of vector 'v' 

def length2(v):

    return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]

# Calculates vector dot product : 'v1' * 'v2'   

def dot(v1, v2):

    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]

# Calculates vector cross product : 'v1' x 'v2'

def cross(v1, v2):

    return (v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2], v1[0] * v2[1] - v1[1] * v2[0])

# Normalizes vector 'v'

def normalize_vec(v):

    l2 = length2(v)

    if l2 == 0:

        return (0, 0, 0)

    l = math.sqrt(l2)

    return (v[0] / l, v[1] / l, v[2] / l)