from pyglet.gl import *
from math import sqrt, acos

def cross(a, b):
    return (a[1]*b[2] - a[2]*b[1],
            a[2]*b[0] - a[0]*b[2],
            a[0]*b[1] - a[1]*b[0])

def dot(a, b):
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]

def mag(a):
    return sqrt(a[0]**2 + a[1]**2 + a[2]**2)    

def norm(a):
    m = mag(a)
    return (a[0]/m, a[1]/m, a[2]/m)

def get_sphere_mapping(x, y, width, height):
    x = min([max([x,0]), width])
    y = min([max([y,0]), height])

    sr = sqrt( (width/2)**2 + (height/2)**2 )
    #sr *= 1.5
    sx = ( (x - width/2)  / sr )
    sy = ( (y - height/2) / sr )

    sz = 1.0 - sx**2 - sy**2

    if sz > 0.0:
        sz = sqrt(sz)
        return (sx, sy, sz)
    else:
        sz = 0
        return norm( (sx, sy, sz) )

rad2deg = 180.0/3.141592

def get_spherical_rotatation(p1, p2, width, height, theta_multiplier):
    v1 = get_sphere_mapping(p1[0], p1[1], width, height)
    v2 = get_sphere_mapping(p2[0], p2[1], width, height)

    d = min(max([dot(v1, v2), -1]), 1)

    if abs(d - 1.0) < 0.000001:
        return None

    raxis = norm( cross(v1, v2) )
    rtheta = theta_multiplier * rad2deg * acos(d)
    #rtheta = 2.0 * rad2deg * acos(d)

    glPushMatrix()
    glLoadIdentity()
    glRotatef(rtheta, *raxis)
    mat = (c_float*16)()
    glGetFloatv(GL_MODELVIEW_MATRIX, mat)
    glPopMatrix()

    return mat
