#!/usr/bin/env python
#
# Copyright (C) 2006 Chris Skalenda <chris@atld.org>
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
#

import string

# for sin, cos, etc.
from math import *

#imports to get OpenGL and other things we need
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *

import sys

name = 'Imagine it\'s a Borg cube or something. Scary!'

rotate_x = 0
rotate_y = 0
rotate_z = 0 
coords_x = 0
coords_y = 0
coords_z = 0
mouse_x = 0
mouse_y = 0
mouse_z = 0
mouse_start_x = 0
mouse_start_y = 0
coord_last_x = 0
coord_last_y = 0
azimuth_x = 10
azimuth_z = 10
polar = 0
azimuth = 0
incidence = 0
last_quat = []

key_points = []

small_step_number = 20

def main():
    readFile()
    glutInit(sys.argv)
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH)
    glutInitWindowSize(800,800)
    glutCreateWindow(name)

    glClearColor(0.,0.,0.,1.)
    glEnable(GL_CULL_FACE)

    # Setup callback functions
    glutDisplayFunc(display)
    glutKeyboardFunc(processKeys)
    glutMouseFunc(processMouse)

    glMatrixMode(GL_PROJECTION)
    gluPerspective(80.,1.,1.,200.)
    glMatrixMode(GL_MODELVIEW)
    glutMainLoop()
    return


def display():
    global rotate_x, rotate_y, rotate_z 
    global coords_x, coords_y, coords_z 
    global mouse_x, mouse_y, mouse_z
    global azimuth_x, azimuth_z, incidence
    global azimuth
    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)


    glPushMatrix()
    # Translate based on the accumulated mouse movements
    glTranslatef(mouse_x*-.1,mouse_y*.1,mouse_z*.1)

    # Translate based on keyboard input
    #glTranslatef(coords_x,coords_y,coords_z)

    # The azimuth rotation is handled by rotating the camera location,
    # while maintaining the same eye location
    gluLookAt(azimuth_x, 0, azimuth_z, 
              0, 0, 0, 
              0, 1, 0)

    # Cross the azimuth vector (which is pointed toward the camera)
    # with a unit vector along the y axis to gain a vector orthogonal 
    # to the camera rotation vector in the x-z plane. This allows the 
    # object to rotate directly toward the camera despite changes
    # in the azimuth vector.
    glRotatef(-incidence,
              0*0 - cos(azimuth)*1,             # v1y * v2z - v1z * v2y
              cos(azimuth)*0 - sin(azimuth)*0,  # v1z * v2x - v1x * v2z
              sin(azimuth)*1 - 0*0)             # v1x * v2y - v1y * v2x 

    for point_array in key_points:
        glPushMatrix()
        glColor3f(1., 0., 0.)
        glTranslatef(point_array[0],point_array[1],point_array[2])
        glutWireSphere(1,10,10)
        glPopMatrix()

    drawCube()
    drawAxis()

    glPopMatrix()
    glutSwapBuffers()
    return

# Linear Interpolation with instantaneous acceleration.
def lInterpI(initial_frame, next_frame):
    global azimuth_x, azimuth_z, incidence
    global azimuth
    global small_step_number
    global last_quat

    ix = initial_frame[0] 
    iy = initial_frame[1] 
    iz = initial_frame[2] 
    fx = next_frame[0] 
    fy = next_frame[1] 
    fz = next_frame[2]  

    current_step = 0
    current_small_step = 0

    # the distance the object needs to travel.
    magnitude = sqrt(pow(fx-ix,2) + pow(fy-iy,2) + pow(fz-iz,2))
    
    # divide this into 1 timestep per one unit traveled and a partial step
    whole_steps = floor(magnitude)
    partial_step_length = magnitude % floor(magnitude)

    # This will interate through each whole step (i) (being one unit)
    # by subdividing it into a set of smaller steps (j) 
    for i in range(0,int(whole_steps)):
        for j in range(0,small_step_number):
            
            glPushMatrix()

            glTranslatef(mouse_x*-.1,mouse_y*.1,mouse_z*.1)
            gluLookAt(azimuth_x, 0, azimuth_z, 
                      0, 0, 0, 
                      0, 1, 0)
            glRotatef(-incidence,
                      0*0 - cos(azimuth)*1,             # v1y * v2z - v1z * v2y
                      cos(azimuth)*0 - sin(azimuth)*0,  # v1z * v2x - v1x * v2z
                      sin(azimuth)*1 - 0*0)             # v1x * v2y - v1y * v2x 

            for point_array in key_points:
                glPushMatrix()
                glTranslatef(point_array[0],point_array[1],point_array[2])
                glRotatef(j,1,0,0)
                glutWireSphere(1,10,10)
                glPopMatrix()
                
            glTranslatef(ix,iy,iz)

            # whole step length + partial step length
            glTranslatef(((fx-ix)*(i/whole_steps) + ((fx-ix)/whole_steps)*(j/float(small_step_number))), 
                         ((fy-iy)*(i/whole_steps) + ((fy-iy)/whole_steps)*(j/float(small_step_number))), 
                         ((fz-iz)*(i/whole_steps) + ((fz-iz)/whole_steps)*(j/float(small_step_number))))


            quat = slerp(initial_frame[3:], next_frame[3:],
                        ((small_step_number*i+j)/float(small_step_number*whole_steps)))
            
            scale = sqrt(pow(quat[1],2)+pow(quat[2],2)+pow(quat[3],2))
            glRotatef(2*acos(quat[0]),quat[1]/scale,quat[2]/scale,quat[3]/scale)

            drawCube()
            drawAxis()
            glPopMatrix()
            glutSwapBuffers()
            glClear (GL_COLOR_BUFFER_BIT)
            
        
# Linear Interpolation with instantaneous acceleration.
def lInterpE(initial_frame, next_frame):
    global azimuth_x, azimuth_z, incidence
    global azimuth
    global small_step_number
    global last_quat

    ix = initial_frame[0] 
    iy = initial_frame[1] 
    iz = initial_frame[2] 
    fx = next_frame[0] 
    fy = next_frame[1] 
    fz = next_frame[2]  

    current_step = 0
    current_small_step = 0

    local_step = small_step_number

    # the distance the object needs to travel.
    magnitude = sqrt(pow(fx-ix,2) + pow(fy-iy,2) + pow(fz-iz,2))
    
    # divide this into 1 timestep per one unit traveled and a partial step
    whole_steps = floor(magnitude)
    partial_step_length = magnitude % floor(magnitude)

    # This will interate through each whole step (i) (being one unit)
    # by subdividing it into a set of smaller steps (j) 
    for i in range(0,int(whole_steps)):
        
        if(initial_frame == key_points[0]):
            local_step = int(abs(small_step_number + small_step_number*(sin(float(i)/whole_steps))))
        elif(next_frame == key_points[len(key_points)-1]):
            local_step = int(abs(small_step_number + small_step_number*(cos(float(i)/whole_steps))))
        else:
            local_step = small_step_number
        for j in range(0,local_step):
            
            glPushMatrix()

            glTranslatef(mouse_x*-.1,mouse_y*.1,mouse_z*.1)
            gluLookAt(azimuth_x, 0, azimuth_z, 
                      0, 0, 0, 
                      0, 1, 0)
            glRotatef(-incidence,
                      0*0 - cos(azimuth)*1,             # v1y * v2z - v1z * v2y
                      cos(azimuth)*0 - sin(azimuth)*0,  # v1z * v2x - v1x * v2z
                      sin(azimuth)*1 - 0*0)             # v1x * v2y - v1y * v2x 

            for point_array in key_points:
                glPushMatrix()
                glTranslatef(point_array[0],point_array[1],point_array[2])
                glRotatef(j,1,0,0)
                glutWireSphere(1,10,10)
                glPopMatrix()
                
            glTranslatef(ix,iy,iz)

            glTranslatef(((fx-ix)*(i/whole_steps) + ((fx-ix)/whole_steps)*(j/float(local_step))), 
                         ((fy-iy)*(i/whole_steps) + ((fy-iy)/whole_steps)*(j/float(local_step))), 
                         ((fz-iz)*(i/whole_steps) + ((fz-iz)/whole_steps)*(j/float(local_step))))

            quat = slerp(initial_frame[3:], next_frame[3:],
                        ((local_step*i+j)/float(local_step*whole_steps)))
            
            scale = sqrt(pow(quat[1],2)+pow(quat[2],2)+pow(quat[3],2))
            glRotatef(2*acos(quat[0]),quat[1]/scale,quat[2]/scale,quat[3]/scale)

            drawCube()
            drawAxis()
            glPopMatrix()
            glutSwapBuffers()
            glClear (GL_COLOR_BUFFER_BIT)
        
def catmull_rom():
    global azimuth_x, azimuth_z, incidence
    global azimuth
    global small_step_number

    # points used in the calculation
    r_p = []
    rom_point = []

    for i in range(0,3):
        rom_point.append(0)
        r_p.append(0);
    
    r_p.append(0);
    end_tracker = 0
    for i in key_points:

        # Break from the loop when we've used the maximum number of points we can.
        if end_tracker == (len(key_points) - 3):
            break

        # Gain the points we want for interpolation this round.
        for j in range(0,4):
            r_p[j] = key_points[end_tracker+j]

        magnitude = sqrt(pow(r_p[1][0]-r_p[2][0],2) + 
                         pow(r_p[1][1]-r_p[2][1],2) + 
                         pow(r_p[1][2]-r_p[2][2],2))

        rom_steps = small_step_number * magnitude

        
        for j in range(1,int(rom_steps)):
            glPushMatrix()

            glTranslatef(mouse_x*-.1,mouse_y*.1,mouse_z*.1)
            gluLookAt(azimuth_x, 0, azimuth_z, 
                      0, 0, 0, 
                      0, 1, 0)
            glRotatef(-incidence,
                      0*0 - cos(azimuth)*1,             # v1y * v2z - v1z * v2y
                      cos(azimuth)*0 - sin(azimuth)*0,  # v1z * v2x - v1x * v2z
                      sin(azimuth)*1 - 0*0)             # v1x * v2y - v1y * v2x 

            for point_array in key_points:
                glPushMatrix()
                glTranslatef(point_array[0],point_array[1],point_array[2])
                glRotatef(j,1,0,0)
                glutWireSphere(1,10,10)
                glPopMatrix()

            rom_point[0] = .5 * ((2*r_p[1][0]) +
                                 (-r_p[0][0]  + r_p[2][0])  * (j/rom_steps) +
                                 (2*r_p[0][0] - 5*r_p[1][0] + 4*r_p[2][0]  - r_p[3][0]) * pow((j/rom_steps),2) +
                                 (-r_p[0][0]  + 3*r_p[1][0]  - 3*r_p[2][0] + r_p[3][0]) * pow((j/rom_steps),3))
            rom_point[1] = .5 * ((2*r_p[1][1]) +
                                 (-r_p[0][1]  + r_p[2][1])  * (j/rom_steps) +
                                 (2*r_p[0][1] - 5*r_p[1][1] + 4*r_p[2][1] - r_p[3][1]) * pow((j/rom_steps),2) +
                                 (-r_p[0][1]  + 3*r_p[1][1] - 3*r_p[2][1] + r_p[3][1]) * pow((j/rom_steps),3))
            rom_point[2] = .5 * ((2*r_p[1][2]) +
                                 (-r_p[0][2]  + r_p[2][2])  * (j/rom_steps) +
                                 (2*r_p[0][2] - 5*r_p[1][2] + 4*r_p[2][2] - r_p[3][2]) * pow((j/rom_steps),2) +
                                 (-r_p[0][2]  + 3*r_p[1][2] - 3*r_p[2][2] + r_p[3][2]) * pow((j/rom_steps),3))

            glTranslatef(rom_point[0],rom_point[1],rom_point[2])

            quat = slerp(r_p[1][3:], r_p[2][3:],
                        (j)/float(rom_steps))
            
            scale = sqrt(pow(quat[1],2)+pow(quat[2],2)+pow(quat[3],2))
            glRotatef(2*acos(quat[0]),quat[1]/scale,quat[2]/scale,quat[3]/scale)

            drawCube()
            drawAxis()
            glPopMatrix()
            glutSwapBuffers()
            glClear (GL_COLOR_BUFFER_BIT)
             

        end_tracker = end_tracker + 1 
    
def b_spline():
    global azimuth_x, azimuth_z, incidence
    global azimuth
    global small_step_number

    # points used in the calculation
    bs_p = []
    spline_point = []

    for i in range(0,3):
        spline_point.append(0)
        bs_p.append(0);
    
    bs_p.append(0);
    end_tracker = 0
    for i in key_points:

        # Break from the loop when we've used the maximum number of points we can.
        if end_tracker == (len(key_points) - 3):
            break

        # Gain the points we want for interpolation this round.
        for j in range(0,4):
            bs_p[j] = key_points[end_tracker+j]

        magnitude = sqrt(pow(bs_p[1][0]-bs_p[2][0],2) + 
                         pow(bs_p[1][1]-bs_p[2][1],2) + 
                         pow(bs_p[1][2]-bs_p[2][2],2))

        spline_steps = small_step_number * magnitude

        
        for j in range(1,int(spline_steps)):
            glPushMatrix()

            glTranslatef(mouse_x*-.1,mouse_y*.1,mouse_z*.1)
            gluLookAt(azimuth_x, 0, azimuth_z, 
                      0, 0, 0, 
                      0, 1, 0)
            glRotatef(-incidence,
                      0*0 - cos(azimuth)*1,             # v1y * v2z - v1z * v2y
                      cos(azimuth)*0 - sin(azimuth)*0,  # v1z * v2x - v1x * v2z
                      sin(azimuth)*1 - 0*0)             # v1x * v2y - v1y * v2x 

            for point_array in key_points:
                glPushMatrix()
                glTranslatef(point_array[0],point_array[1],point_array[2])
                glRotatef(j,1,0,0)
                glutWireSphere(1,10,10)
                glPopMatrix()

            spline_point[0] = ((1./6.) * (-bs_p[0][0] + 3*bs_p[1][0] - 3*bs_p[2][0] + bs_p[3][0]) * pow((j/spline_steps),3) +
                               (1./2.) * ( bs_p[0][0] - 2*bs_p[1][0] + bs_p[2][0]) * pow((j/spline_steps),2) +
                               (1./2.) * (-bs_p[0][0] +   bs_p[2][0])  * (j/spline_steps) +
                               (1./6.) * ( bs_p[0][0] + 4*bs_p[1][0] +   bs_p[2][0]))

            spline_point[1] = ((1./6.) * (-bs_p[0][1] + 3*bs_p[1][1] - 3*bs_p[2][1] + bs_p[3][1]) * pow((j/spline_steps),3) +
                               (1./2.) * ( bs_p[0][1] - 2*bs_p[1][1] +   bs_p[2][1]) * pow((j/spline_steps),2) +
                               (1./2.) * (-bs_p[0][1] +   bs_p[2][1]) * (j/spline_steps) +
                               (1./6.) * ( bs_p[0][1] + 4*bs_p[1][1] +   bs_p[2][1]))

            spline_point[2] = ((1./6.) * (-bs_p[0][2] + 3*bs_p[1][2] - 3*bs_p[2][2] + bs_p[3][2]) * pow((j/spline_steps),3) +
                               (1./2.) * ( bs_p[0][2] - 2*bs_p[1][2] +   bs_p[2][2]) * pow((j/spline_steps),2) +
                               (1./2.) * (-bs_p[0][2] +   bs_p[2][2]) * (j/spline_steps) +
                               (1./6.) * ( bs_p[0][2] + 4*bs_p[1][2] +   bs_p[2][2]))

            glTranslatef(spline_point[0],spline_point[1],spline_point[2])

            quat = slerp(bs_p[1][3:], bs_p[2][3:],
                        (j)/float(spline_steps))
            
            scale = sqrt(pow(quat[1],2)+pow(quat[2],2)+pow(quat[3],2))
            glRotatef(2*acos(quat[0]),quat[1]/scale,quat[2]/scale,quat[3]/scale)

            drawCube()
            drawAxis()
            glPopMatrix()
            glutSwapBuffers()
            glClear (GL_COLOR_BUFFER_BIT)
        
        end_tracker = end_tracker + 1 

def processKeys(key, x, y):
    global rotate_x, rotate_y, rotate_z, coords_x, coords_y, coords_z, polar

    if key == 'l':
        j = 1
        for i in key_points:
            if j == len(key_points):
                break
            lInterpI(i,key_points[j])
            j = j+1
    elif key == 'r':
        catmull_rom()
    elif key == 'b':
        b_spline()
    elif key == 'e':
        j = 1
        for i in key_points:
            if j == len(key_points):
                break
            lInterpE(i,key_points[j])
            j = j+1
    elif key == 'p':
        polar = 1
    elif key == 'c':
        polar = 0

    display()
    return 

def processMouse(button, state, x, y):
    global mouse_start_xr, mouse_start_y
    # Check if button is pressed down, so the motion callbacks can be set
    # accordingly. This way, different callbacks are set for each button
    if state == GLUT_DOWN:
        mouse_start_x = x
        mouse_start_y = y
        if button == 0:
            if polar == 0:
                glutMotionFunc(processLeftMotion)
            elif polar == 1:
                glutMotionFunc(processPolarMotion)
        elif button == 2:
            glutMotionFunc(processRightMotion)
    return 
            

def processLeftMotion(x, y):
    global mouse_x, mouse_y, mouse_z 
    global mouse_start_x, mouse_start_y
    global coord_last_x, coord_last_y

    if coord_last_y == 0:
        coord_last_x = mouse_start_x
        coord_last_y = mouse_start_y

    # Handling x mouse movements 
    if coord_last_x < x:
        mouse_x += 1
    elif coord_last_x > x:
        mouse_x -= 1

    # And the same for y
    if coord_last_y < y:
        mouse_y += 1
    elif coord_last_y > y:
        mouse_y -= 1

    coord_last_x = x
    coord_last_y = y

    display()
    return 

def processRightMotion(x, y):
    global mouse_z, mouse_x, mouse_start_y
    # Zoom!
    mouse_z = y - mouse_start_y
    display()
    return 

def processPolarMotion(x, y):
    global azimuth_x, azimuth_z 
    global mouse_start_x, mouse_start_y
    global coord_last_x, coord_last_y
    global azimuth, incidence
    
    # set the last coordinates initially
    if coord_last_y == 0:
        coord_last_x = mouse_start_x
        coord_last_y = mouse_start_y

    if coord_last_x < x:
        # Add 1 to the camera degree rotated around y
        azimuth -= .05
        
    elif coord_last_x > x:
        # Remove 1 from the camera degree rotated around y
        azimuth += .05

    if coord_last_y < y:
        incidence += 2
    elif coord_last_y > y:
        incidence -= 2
    
    # finding a constant magnitude vector to move the camera to the end of.
    # yay trig!
    azimuth_x = sin(azimuth)*15 
    azimuth_z = cos(azimuth)*15 
    
    coord_last_x = x
    coord_last_y = y
    
    display()
    return

def drawCube():
    # Front
    glBegin(GL_POLYGON)
    glColor3f(.5, .5, 0.)
    glVertex3d(-2,2,2)
    glColor3f(0., .5, .5)
    glVertex3d(-2,-2,2)
    glColor3f(5., 0., .5)
    glVertex3d(2,-2,2)
    glColor3f(5., 5., .2)
    glVertex3d(2,2,2)
    glEnd()

    # Left side
    glBegin(GL_POLYGON)
    glColor3f(.5, .5, 0.)
    glVertex3d(-2,-2,-2)
    glColor3f(0., .5, .5)
    glVertex3d(-2,-2,2)
    glColor3f(5., 0., .5)
    glVertex3d(-2,2,2)
    glColor3f(5., 5., .2)
    glVertex3d(-2,2,-2)
    glEnd()

    # Right side
    glBegin(GL_POLYGON)
    glColor3f(.5, .5, 0.)
    glVertex3d(2,2,-2)
    glColor3f(0., .5, .5)
    glVertex3d(2,2,2)
    glColor3f(5., 0., .5)
    glVertex3d(2,-2,2)
    glColor3f(5., 5., .2)
    glVertex3d(2,-2,-2)
    glEnd()

    # Top
    glBegin(GL_POLYGON)
    glColor3f(.5, .5, 0.)
    glVertex3d(-2,2,-2)
    glColor3f(0., .5, .5)
    glVertex3d(-2,2,2)
    glColor3f(5., 0., .5)
    glVertex3d(2,2,2)
    glColor3f(5., 5., .2)
    glVertex3d(2,2,-2)
    glEnd()

    # Back
    glBegin(GL_POLYGON)
    glColor3f(.5, .5, 0.)
    glVertex3d(-2,2,-2)
    glColor3f(0., .5, .5)
    glVertex3d(2,2,-2)
    glColor3f(5., 0., .5)
    glVertex3d(2,-2,-2)
    glColor3f(5., 5., .2)
    glVertex3d(-2,-2,-2)
    glEnd()

    # Bottom
    glBegin(GL_POLYGON)
    glColor3f(.5, .5, 0.)
    glVertex3d(-2,-2,-2)
    glColor3f(0., .5, .5)
    glVertex3d(2,-2,-2)
    glColor3f(5., 0., .5)
    glVertex3d(2,-2,2)
    glColor3f(5., 5., .2)
    glVertex3d(-2,-2,2)
    glEnd()

def drawAxis():
    # Code stolen from Jarvis to show the object's axis.
    glPushMatrix()
    glBegin(GL_LINES)
    glColor3f(0.,0.,1.)
    glVertex3d(-1000.,0.,0.)
    glVertex3d(1000.,0.,0.)

    glColor3f(0., 1., 0.)
    glVertex3d(0., -1000., 0.) 
    glVertex3d(0., 1000., 0.)

    glColor3f(1., 0., 0.)
    glVertex3d(0., 0.0, -1000.0) 
    glVertex3d(0., 0.0, 1000.0)

    glEnd()
    glPopMatrix()

def slerp(quat_a, quat_b, time):
    quat_z = []
    cos_theta = quat_a[0] * quat_b[0] + quat_a[1] * quat_b[1] +quat_a[2] * quat_b[2] + quat_a[3] * quat_b[3] 
    if(quat_a == quat_b):
        return quat_a
    
    theta = acos(cos_theta);

    if (abs(theta) < 0.01):
        return quat_a
    
    sin_theta = sqrt(1.0-pow(cos_theta,2))
    if (abs(sin_theta) < 0.01):
        quat_z.append(quat_a[0]*.5 + quat_b[0]*.5)
        quat_z.append(quat_a[1]*.5 + quat_b[1]*.5)
        quat_z.append(quat_a[2]*.5 + quat_b[2]*.5)
        quat_z.append(quat_a[3]*.5 + quat_b[3]*.5)
        return quat_z
    
    ratio_a = sin((1-time) * theta) / sin_theta
    ratio_b = sin(time * theta) / sin_theta

    quat_z.append(quat_a[0] * ratio_a + quat_b[0] * ratio_b)
    quat_z.append(quat_a[1] * ratio_a + quat_b[1] * ratio_b)
    quat_z.append(quat_a[2] * ratio_a + quat_b[2] * ratio_b)
    quat_z.append(quat_a[3] * ratio_a + quat_b[3] * ratio_b)

    return quat_z

def mul_quat(quat_a, quat_b):
    quat_z = []
    quat_z.append(quat_a[0]*quat_b[0] - quat_a[1]*quat_b[1] - quat_a[2]*quat_b[2] - quat_a[3]*quat_b[3])
    quat_z.append(quat_a[0]*quat_b[1] + quat_a[2]*quat_b[0] + quat_a[2]*quat_b[3] - quat_a[3]*quat_b[2])
    quat_z.append(quat_a[0]*quat_b[2] + quat_a[2]*quat_b[0] + quat_a[3]*quat_b[1] - quat_a[1]*quat_b[3])
    quat_z.append(quat_a[0]*quat_b[3] + quat_a[3]*quat_b[0] + quat_a[1]*quat_b[2] - quat_a[2]*quat_b[1])
    print quat_z
    if quat_z[0] < -1:
        quat_z[0] = 2 + quat_z[0]
    return quat_z

def readFile():
    instructions()
    f=open('input', 'r')
    for line in f:
        if line.strip().split(" ") != "":
            key_points.append(line.strip().split(" "))

    for i in range(0,len(key_points)):
        for j in range(0,len(key_points[i])):
            key_points[i][j] = float(key_points[i][j])

    for i in range(0,len(key_points)):
        if(len(key_points[i]) == 6):
            key_points[i] = euler_to_quat(key_points[i][4:])
            
def euler_to_quat(euler):
    x = [cos(euler[0]/2.),sin(euler[0]/2.),0,0]
    y = [cos(euler[2]/2.),0,sin(euler[0]/2.),0]
    z = [cos(euler[0]/2.),0,0,sin(euler[0]/2.)]
    return mul_quat(mul_quat(x,y),z)

def instructions():
    print "Usage"
    print "Linter interpolation: l"
    print "Catmull-Rom: r"
    print "B-Splines: b"
    print "Liner ease-in/ease-out: e"
    print
    print "Camera controls"
    print "Polar orientation: p"
    print "Pan orientation: c"
    print "Zoom: right mouse button"
    print "Camera movement: left mouse button"
    print 
    print "File format"
    print "Point coordinates followed by either euler angle or quaternion values"
    print "separated by spaces. Each key frame is on a line."
    print "The anticipated file name is \"input\""
    print "ex:" 
    print "x y z a b c"
    print "or"
    print "x y z w x y z"

if __name__ == '__main__': main()