from math import *


def scal_vec_mul(s,v):
   return [s * v[0],s * v[1],s * v[2]]

def vec_scal_mul(v,s):
   return [s * v[0],s * v[1],s * v[2]]

def vec_add(va,vb):
   return [va[0]+vb[0], va[1]+vb[1], va[2]+vb[2]]

def vec_sub(va,vb):
   return [va[0]-vb[0], va[1]-vb[1], va[2]-vb[2]]

def dotp(va, vb):
    return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2])

def crossp(va,vb):
    return ([va[1]*vb[2] - va[2]*vb[1],va[2]*vb[0] - va[0]*vb[2],va[0]*vb[1] - va[1]*vb[0]])

def vec_len(v):
    return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])

def vec_normal(v):
    l = vec_len(v)
    return [v[0]/l,v[1]/l,v[2]/l]

def transform_dir_to_src(m,v):
    v1 = scal_vec_mul(v[0],m[0])
    v2 = scal_vec_mul(v[1],m[1])
    v3 = scal_vec_mul(v[2],m[2])
    return vec_add(v1,vec_add(v2,v3))

def transform_dir_to_dest(m,v):
    return [dotp(m[0],v), dotp(m[1],v), dotp(m[2],v)]

def transform_point_to_src(m,v):
    p = transform_dir_to_src(m,v)
    return vec_add(p,m[3])

def transform_point_to_dest(m,v):
    vo = vec_sub(v,m[3])
    return transform_dir_to_dest(m,vo)

def transform_matrix_to_src(m,mt):
    return [transform_dir_to_src(m,mt[0]), transform_dir_to_src(m,mt[1]), transform_dir_to_src(m,mt[2]), transform_point_to_src(m,mt[3])]

def transform_matrix_to_dest(m,mt):
    return [transform_dir_to_dest(m,mt[0]), transform_dir_to_dest(m,mt[1]), transform_dir_to_dest(m,mt[2]), transform_point_to_dest(m,mt[3])]

def matrix_advance(m,alpha):
    m[3] = vec_add(m[3],vec_scal_mul(m[1],alpha))

def matrix_rotate_about_side(m,alpha):
    ca = cos(alpha)
    sa = sin(alpha)
    new_f = vec_add(vec_scal_mul(m[1],ca) , vec_scal_mul(m[2],sa))
    new_u = vec_sub(vec_scal_mul(m[2],ca) , vec_scal_mul(m[1],sa))
    m[1][0], m[1][1],m[1][2] = new_f
    m[2][0], m[2][1],m[2][2] = new_u

def matrix_rotate_about_forward(m,alpha):
    ca = cos(alpha)
    sa = sin(alpha)
    new_s = vec_sub(vec_scal_mul(m[0],ca) , vec_scal_mul(m[2],sa))
    new_u = vec_add(vec_scal_mul(m[2],ca) , vec_scal_mul(m[0],sa))
    m[0] = new_s
    m[2] = new_u

def matrix_rotate_about_up(m,alpha):
    ca = cos(alpha)
    sa = sin(alpha)
    new_s = vec_add(vec_scal_mul(m[0],ca) , vec_scal_mul(m[1],sa))
    new_f = vec_sub(vec_scal_mul(m[1],ca) , vec_scal_mul(m[0],sa))
    m[0] = new_s
    m[1] = new_f

def identityMatrix4():
    return [[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0],[0.0, 0.0, 1.0, 0.0],[0.0, 0.0, 0.0, 1.0]]

def identityMatrix3():
    return [[1.0, 0.0, 0.0],[0.0, 1.0, 0.0],[0.0, 0.0, 1.0]]
