#!/usr/bin/python
import math

def angle_between_vectors(u, v, n):
    up = project_plane(u, n)
    vp = project_plane(v, n)
    uv = crossproduct(up, vp)
    return math.atan2(dot(n, uv), dot(up, vp))

def project_plane(u, n):
    un = project(u, n)
    return vecsub(u, un)

def project(u, v):
    vnorm, _= unitize(v)
    return vecscalarmult(vnorm, dot(u,vnorm))

def unitize(u):
    f = u[0]*u[0] + u[1]*u[1] + u[2]*u[2]
    
    if f != 0.0:
        m = math.sqrt(f)
        u[0] /= m
        u[1] /= m
        u[2] /= m
    else:
        m = 0
    return (u, m)

def vecscalarmult(v, f):
    u = list(v)[:]
    u[0] = v[0] * f
    u[1] = v[1] * f
    u[2] = v[2] * f
    return u

def vecsub(u, v):
    t = list(u)[:]
    t[0] = u[0] - v[0]
    t[1] = u[1] - v[1]
    t[2] = u[2] - v[2]
    return t

def crossproduct(a, b):
    r = list(a)[:]
    r[0] = a[1] * b[2] - a[2] * b[1]
    r[1] = a[2] * b[0] - a[0] * b[2]
    r[2] = a[0] * b[1] - a[1] * b[0]
    return r

def dot(u,v):
    return (u[0]*v[0] + u[1]*v[1] + u[2]*v[2])


def rotation_axis_to_matrix(axis, angle):
    r = [[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0]]
    cos_a = math.cos(angle)
    sin_a = math.sin(angle)

    s1 = axis[0]
    s2 = axis[1]
    s3 = axis[2]
    s1s1 = s1*s1
    s1s2 = s1*s2
    s1s3 = s1*s3
    s2s2 = s2*s2
    s2s3 = s2*s3
    s3s3 = s3*s3
    cos_as1s2 = cos_a * s1s2
    cos_as1s3 = cos_a * s1s3
    cos_as2s3 = cos_a * s2s3
    s1sin_a = s1*sin_a
    s2sin_a = s2*sin_a
    s3sin_a = s3*sin_a

    r[0][0] = s1s1 + cos_a * (1 - s1s1)
    r[0][1] = s1s2 - cos_as1s2 + s3sin_a
    r[0][2] = s1s3 - cos_as1s3 - s2sin_a
    r[0][3] = 0

    r[1][0] = s1s2 - cos_as1s2 - s3sin_a
    r[1][1] = s2s2 + cos_a * (1 - s2s2)
    r[1][2] = s2s3 - cos_as2s3 + s1sin_a
    r[1][3] = 0

    r[2][0] = s1s3 - cos_as1s3 + s2sin_a
    r[2][1] = s2s3 - cos_as2s3 - s1sin_a
    r[2][2] = s3s3 + cos_a * (1 - s3s3)
    r[2][3] = 0

    r[3][0] = 0
    r[3][1] = 0
    r[3][2] = 0
    r[3][3] = 1
    return r


def rotation_matrix_to_axis(R):
    eps = 1e-7
    axis = [0.0, 0.0, 0.0]
    angle = math.acos((R[0][0] + R[1][1] + R[2][2] - 1) / 2.0)

    if (math.fabs(angle) < eps or math.fabs(angle - math.pi) < eps):
        angle = 0.0
        axis[0] = 0.0
        axis[1] = 0.0
        axis[2] = 1.0
    else:
        axis[0] = R[1][2] - R[2][1]
        axis[1] = R[2][0] - R[0][2]
        axis[2] = R[0][1] - R[1][0]
        unitize(axis)
    return axis, angle


import sys, numpy
if __name__ == '__main__':
    #a = numpy.array([[1,0,0,22],[0,1,0,12],[0,0,1,14],[0,0,0,1]])
    bb = [0.0, 0.0, 1.0], -1.5707963267948966
    cc = [0.99594269821, -0.0186375672606, -0.0880385311584], 0.78539816339744828
    b = numpy.array(rotation_axis_to_matrix(*bb ))
    c = numpy.array(rotation_axis_to_matrix(*cc))
    d = numpy.dot(b,c)
    def test(a):
        axis, angle = rotation_matrix_to_axis(a)
        axis, m = unitize(axis)
        angle *= m
        print "%r %r" % (axis, angle)
    test(b)
    test(c)
    test(d)

    bba = map(lambda a:math.degrees(bb[1]*a), bb[0])
    cca = map(lambda a:math.degrees(cc[1]*a), cc[0])
    
    print "%r" % (bba,)
    print "%r" % (cca,)
    dda = [ bba[i]+cca[i] for i in range(len(bba)) ]
    print "%r" % (dda,)
    dda = map(math.radians, dda)
    dda, m = unitize(dda)
    print "%r %r" % (dda,m)

    sys.exit(0)
    a = [1.0,  0.0, 0.0]
    b = [9.0,  0.0, 0.1]
    
    
    x = angle_between_vectors(a, b, [1.0,  0.0, 0.0])
    y = angle_between_vectors(a, b, [0.0,  1.0, 0.0])
    z = angle_between_vectors(a, b, [0.0,  0.0, 1.0])
    print math.degrees(x),math.degrees(y),math.degrees(z)
    
    N = crossproduct(a,b)
    N, _ = unitize(N)
    angle = angle_between_vectors(a, b, N)
    print map(lambda a:a*math.degrees(angle),N)
    #rotation_axis_to_matrix(N, angle, S0)