Added plot_lighthouse in dave folder

9 files changed, 776 insertions, 0 deletions

diff --git a/dave/plot_lighthouse/.DS_Store b/dave/plot_lighthouse/.DS_Store
new file mode 100644
index 0000000..493ea1e
--- /dev/null
+++ b/dave/plot_lighthouse/.DS_Store
diff --git a/dave/plot_lighthouse/Makefile b/dave/plot_lighthouse/Makefile
new file mode 100644
index 0000000..d156bdb
--- /dev/null
+++ b/dave/plot_lighthouse/Makefile
@@ -0,0 +1,15 @@
+UNAME := $(shell uname)
+
+ifeq ($(UNAME), Linux)
+CFLAGS:= -lGL -lGLU -lglut
+endif
+
+# Darwin is Mac OSX !!
+ifeq ($(UNAME), Darwin)
+CFLAGS:= -w -framework OpenGL -framework GLUT
+endif
+
+all:
+	gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c $(CFLAGS)
+clean:
+	rm -f plot_lighthouse
diff --git a/dave/plot_lighthouse/README.txt b/dave/plot_lighthouse/README.txt
new file mode 100644
index 0000000..4e65688
--- /dev/null
+++ b/dave/plot_lighthouse/README.txt
@@ -0,0 +1,20 @@
+Plots the lighthouse positions, and the hmd bearing angles from stdin using OpenGL
+
+----
+- Step 1:
+----
+
+You must run lighthousefind first
+
+lighthousefind L processed_data.txt > L.txt 
+lighthousefind R processed_data.txt > R.txt
+
+you must move L.txt and R.txt into the same directory as this program
+
+----
+- Step 2
+----
+
+Then you need to run processrawcap and pipe the output into plot_lighthouse
+
+../../tools/process_rawcap/process_to_points [raw data.csv] | ./plot_lighthouse
diff --git a/dave/plot_lighthouse/fileutil.c b/dave/plot_lighthouse/fileutil.c
new file mode 100644
index 0000000..f892798
--- /dev/null
+++ b/dave/plot_lighthouse/fileutil.c
@@ -0,0 +1,124 @@
+#include "fileutil.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+#define PI 3.14159265358979323846264
+
+
+og_mutex_t  read_mutex;
+og_thread_t read_thread;
+double read_hmdAngles[NUM_SWEEP][NUM_HMD];
+int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD];
+int read_frameno=0;
+
+static FILE *fopen_orDie(const char *path, const char *flag)
+{
+	FILE *f = fopen(path, flag);
+	if (f == NULL) {
+		printf("ERROR could not oepn %s for %s\n", path, flag);
+		exit(1);
+	}
+	return f;
+}
+
+static void SeekToken(FILE *f, const char *keyword)
+{
+	char token[4096];
+	do {
+		fscanf(f, "%s", token);
+	} while( strcmp(token,keyword)!=0 && !feof(f) );
+}
+
+void LoadLighthousePos(
+        const char *path,
+        float *x, float *y, float *z,
+        float *qi, float *qj, float *qk, float *qreal)
+{
+	FILE *f = fopen_orDie(path,"r");
+	SeekToken(f, "POS:");
+	fscanf(f, "%f %f %f\n", x, y, z);
+	SeekToken(f, "QUAT:");
+	fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk);
+	fclose (f);
+}
+
+void LoadHmdProcessedDataAngles(
+	const char *path,
+	double angles[NUM_SWEEP][NUM_HMD])
+{
+	int i,j;
+	char   type[256];
+	char   sweep[256];
+	int    id;
+	int    nSweep;
+	double ang;
+	double d1,d2,d3;  // revisit these numbers later
+
+	// Initialize all of the angles to -9999
+	for (i=0; i<NUM_SWEEP; i++) {
+		for (j=0; j<NUM_HMD; j++) {
+			angles[i][j] = -9999.0;   // Initially no value
+		}
+	}
+
+	FILE *f = fopen_orDie(path, "r");
+
+	while (!feof(f))
+	{
+		// Read the line from the file
+		int rt=fscanf(f, "%s %s %d %d %lf %lf %lf %lf",
+			&type, &sweep, &id, &nSweep, &ang,
+			&d1,&d2,&d3);
+
+		if (rt<8) { break; }
+
+		// Only hmd points
+		if ( strcmp(type,"HMD")!=0 ) { continue; }
+
+		// Which sweep is it?
+		int sweepId=-1;
+		if      ( strcmp(sweep,"LX")==0 ) { sweepId=SWEEP_LX; }
+		else if ( strcmp(sweep,"LY")==0 ) { sweepId=SWEEP_LY; }
+		else if ( strcmp(sweep,"RX")==0 ) { sweepId=SWEEP_RX; }
+		else if ( strcmp(sweep,"RY")==0 ) { sweepId=SWEEP_RY; }
+		else { continue; }
+		
+		// Convert the angle from ticks to radians
+		angles[sweepId][id] = (PI / 400000.0) * ( ang-200000.0 );
+	}
+
+	fclose(f);
+}
+
+void *ThreadReadHmtAngles(void *junk)
+{
+	char house[256];
+	char xy[256];
+	char hmd[256];
+	double ts;
+	int id;
+	int syncid;
+	int timeInSweep;
+	int length;
+	//lighthouse sweep hmdOrwmd timestamp id syncid timeinsweep length
+
+	while(1) {
+		scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+
+		int sweepId=0;
+		if ( house[0]=='R' ) { sweepId+=2; }
+		if ( xy[0]   =='Y' ) { sweepId++; }
+		double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 );
+		
+		if ( strcmp(hmd,"HMD")!=0 ) { continue; }
+		
+		if ( id<0 || id >NUM_HMD) { continue; }
+
+		OGLockMutex  (read_mutex);
+		read_hmdAngles[sweepId][id] = angle;
+		OGUnlockMutex(read_mutex);
+		read_hmdAngleViewed[sweepId][id] = read_frameno;
+	}
+}
+
+
diff --git a/dave/plot_lighthouse/fileutil.h b/dave/plot_lighthouse/fileutil.h
new file mode 100644
index 0000000..714fbbd
--- /dev/null
+++ b/dave/plot_lighthouse/fileutil.h
@@ -0,0 +1,34 @@
+#ifndef _fileutil_h_
+#define _fileutil_h_
+
+#include <pthread.h>
+#include "../../redist/os_generic.h"
+
+void LoadLighthousePos(
+	const char *path, 
+	float *x, float *y, float *z,
+	float *qi, float *qj, float *qk, float *qreal);
+
+
+#define NUM_HMD   32
+#define NUM_SWEEP 4
+#define SWEEP_LX  0
+#define SWEEP_LY  1
+#define SWEEP_RX  2
+#define SWEEP_RY  3
+void LoadHmdProcessedDataAngles(
+	const char *path,
+	double angle[NUM_SWEEP][NUM_HMD]);
+
+
+extern og_mutex_t  read_mutex;
+extern og_thread_t read_thread;
+extern double read_hmdAngles[NUM_SWEEP][NUM_HMD];
+extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD];
+extern int read_frameno;
+void *ThreadReadHmtAngles(void *junk);
+
+
+#endif // __fileutil_h_
+
+
diff --git a/dave/plot_lighthouse/glutil.c b/dave/plot_lighthouse/glutil.c
new file mode 100644
index 0000000..dd022a0
--- /dev/null
+++ b/dave/plot_lighthouse/glutil.c
@@ -0,0 +1,75 @@
+//
+//  glutil.c
+//  
+//
+//  Created by user on 2/4/17.
+//
+//
+
+#include "glutil.h"
+
+void DrawGrid(
+	float minX, float maxX,
+	float minY, float maxY,
+	float minZ, float maxZ,
+	float stepX, float stepY, float stepZ)
+{
+	float x,y,z;
+	
+	glBegin(GL_LINES);
+
+	// X grid stripes
+	for (y=minY; y<maxY; y+=stepY) {
+		for (z=minZ; z<maxZ; z+=stepZ) {
+			glVertex3f(minX, y, z);
+			glVertex3f(maxX, y, z);
+		}
+	}
+
+	// Y grid stripes
+	for (x=minX; x<maxX; x+=stepX) {
+		for (z=minZ; z<maxZ; z+=stepZ) {
+			glVertex3f(x, minY, z);
+			glVertex3f(x, maxY, z);
+		}
+	}
+
+	// Z grid stripes
+	for (y=minY; y<maxY; y+=stepY) {
+		for (x=minX; x<maxX; x+=stepX) {
+			glVertex3f(x, y, minZ);
+			glVertex3f(x, y, maxZ);
+		}
+	}
+
+	glEnd();
+}
+
+
+void DrawCoordinateSystem(
+	float x, float y, float z,
+	float qx, float qy, float qz, float qr)
+{
+	Quaternion i0,j0,k0;
+	Quaternion i, j, k;
+	Quaternion q;
+
+	// Calculate the i, j, and k vectors
+	QuaternionSet(i0, 1, 0, 0, 0);
+	QuaternionSet(j0, 0, 1, 0, 0);
+	QuaternionSet(k0, 0, 0, 1, 0);
+	QuaternionSet(q, qx, qy, qz, qr);
+	QuaternionRot(i, q, i0);
+	QuaternionRot(j, q, j0);
+	QuaternionRot(k, q, k0);
+	
+	// Draw the coordinate system i red, j green, k blue
+	glBegin(GL_LINES);
+	glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i.i,z+i.k,y+i.j);
+	glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j.i,z+j.k,y+j.j);
+	glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k.i,z+k.k,y+k.j);
+	glEnd();
+}
+
+
+
diff --git a/dave/plot_lighthouse/glutil.h b/dave/plot_lighthouse/glutil.h
new file mode 100644
index 0000000..1014506
--- /dev/null
+++ b/dave/plot_lighthouse/glutil.h
@@ -0,0 +1,34 @@
+//
+//  glutil.h
+//  
+//
+//  Created by user on 2/4/17.
+//
+//
+
+#ifndef glutil_h
+#define glutil_h
+
+#include <stdio.h>
+#include "quaternion.h"
+
+#include <stdio.h>
+#ifdef __APPLE__
+#include <OpenGL/gl.h>   // The GL Header File
+#include <GLUT/glut.h>   // The GL Utility Toolkit (Glut) Header
+#else
+#include <GL/gl.h>
+#include <GL/glu.h>
+#endif
+
+void DrawGrid(
+	float minX, float maxX,
+	float minY, float maxY,
+	float minZ, float maxZ,
+	float stepX, float stepY, float stepZ);
+
+void DrawCoordinateSystem(
+	float x, float y, float z,
+	float qx, float qy, float qz, float qr);
+
+#endif /* glutil_h */
diff --git a/dave/plot_lighthouse/main.c b/dave/plot_lighthouse/main.c
new file mode 100755
index 0000000..c2fd97a
--- /dev/null
+++ b/dave/plot_lighthouse/main.c
@@ -0,0 +1,336 @@
+/*
+ When creating your project, uncheck OWL,
+ uncheck Class Library, select Static
+ instead of Dynamic and change the target
+ model to Console from GUI.
+ Also link glut.lib to your project once its done.
+ */
+
+#include <stdio.h>   // Standard Header For Most Programs
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include "glutil.h"
+#include "fileutil.h"
+
+// Required to set up a window
+#define WIDTH  800
+#define HEIGHT 600
+#define FULLSCREEN 0
+int    keys[256];               // Regular keyboard keys
+int sp_keys[256];               // Special keyboard keycodes (GLUT specific)
+
+#define PI 3.1415926535897932386264
+#define MOVESPEED 1.0
+#define ROTSPEED  5.0
+
+#define GRID_SIZE       6
+#define GRID_MAJOR_STEP 3
+#define GRID_MINOR_STEP 1
+
+// Lighthouses
+typedef struct Lhouse {
+	float x,y,z,qi,qj,qk,qreal;   // position x,y,z and quaternion
+} Lhouse;
+Lhouse houseL, houseR;
+
+// HmdSensor array
+double hmdAngles[NUM_SWEEP][NUM_HMD];
+
+
+static void HmdDir(double LX, double LY, double *pX, double *pY, double *pZ) {
+	// Normals for the two planes
+	double a = cos(LX);  double b = 0.0;     double c = -sin(LX);  // X,Z plane
+	double A = 0.0;      double B = cos(LY); double C = -sin(LY);  // Y,Z plane
+//	double A=0.0; double B=1.0; double C=0.0;
+	printf("rotX %f rotY %f x,z plane normal %f %f %f  y,z plane normal %f %f %f\n",
+		180/PI*LX,180/PI*LY, a,b,c, A,B,C);
+	
+	// calculate the direction
+	double x = (b*C - B*c) / (B*a - A*b);
+	double y = (a*C - A*c) / (A*b - B*a);
+	double z = 1.0;
+	
+	// Normalize
+	double len = sqrt(x*x + y*y + z*z);
+	x/=len; y/=len; z/=len;
+	printf("   xyz %f %f %f\n", x, y, z);
+	
+	*pX=x; *pY=y; *pZ=z;
+}
+
+// View space
+float posx=0.0f;
+float posy=0.0f;
+float posz=0.0f;
+float rotx=0.0f;
+float roty=0.0f;
+float rotz=0.0f;
+
+/*
+ * init() is called at program startup
+ */
+void init()
+{    
+	int i,j;
+    
+	// Load the lighthouses
+	LoadLighthousePos("L.txt", &houseL.x,&houseL.y,&houseL.z,&houseL.qi,&houseL.qj,&houseL.qk,&houseL.qreal);
+	LoadLighthousePos("R.txt", &houseR.x,&houseR.y,&houseR.z,&houseR.qi,&houseR.qj,&houseR.qk,&houseR.qreal);
+	printf("L pos %f %f %f quat %f %f %f %f\n", houseL.x,houseL.y,houseL.z,houseL.qi,houseL.qj,houseL.qk,houseL.qreal);
+	printf("R pos %f %f %f quat %f %f %f %f\n", houseR.x,houseR.y,houseR.z,houseR.qi,houseR.qj,houseR.qk,houseR.qreal);
+/*
+	// Load the bearing angles
+	LoadHmdProcessedDataAngles("processed_data.txt", hmdAngles);
+*/
+	// Spawn the thread to read the hmt angles
+	memset(read_hmdAngleViewed, 0, NUM_HMD*NUM_SWEEP*sizeof(int));
+	read_mutex  = OGCreateMutex();
+	for (i=0; i<32; i++) {
+		for (j=0; j<4; j++) {
+			read_hmdAngles[j][i] = -9999.0;
+		}
+	}
+	read_thread = OGCreateThread(ThreadReadHmtAngles,NULL);	
+
+	for (i=0; i<32; i++) {
+		if (hmdAngles[0][i]!=-9999.0 && hmdAngles[1][i]!=-9999.0 && hmdAngles[2][i]!=-9999.0 && hmdAngles[3][i]!=-9999.0)
+		{
+			printf("hmd %d lx %f ly %f rx %f ry %f\n",
+				i,
+				(180.0/PI) * hmdAngles[SWEEP_LX][i],
+				(180.0/PI) * hmdAngles[SWEEP_LY][i],
+				(180.0/PI) * hmdAngles[SWEEP_RX][i],
+				(180.0/PI) * hmdAngles[SWEEP_RY][i]);
+		}
+	}
+
+	// Initialize OpenGL
+	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
+	glClearColor(0.6f, 0.8f, 1.0f, 0.5f);				// Black Background
+	glClearDepth(1.0f);									// Depth Buffer Setup
+	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
+	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
+	glEnable ( GL_COLOR_MATERIAL );
+	glDisable(GL_CULL_FACE);
+	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
+}
+
+/*
+ * draw() is called once every frame
+ */
+void draw()
+{
+	int i,j;
+
+    //------------------------
+    // Check for keyboard input
+    //------------------------
+    if (keys['w'] || keys['W']) {
+        posz += MOVESPEED;
+    }
+    if (keys['s'] || keys['S']) {
+        posz -= MOVESPEED;
+    }
+    if (keys['a'] || keys['A']) {
+        roty += ROTSPEED;
+    }
+    if (keys['d'] || keys['D']) {
+        roty -= ROTSPEED;
+    }
+    if (keys[27]) {
+        exit(0);
+    }
+    
+    //------------------------
+    // Update the scene
+    //------------------------
+    
+    //------------------------
+    // Draw using OpenGL
+    //------------------------
+    
+    // Clear the screen
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
+ 
+    // Translate and rotate the camera
+    glLoadIdentity();  // Reset The Current Modelview Matrix
+    glTranslatef(-posx,-posy,posz);						// Move Left 1.5 Units And Into The    
+
+    glRotatef(-rotx, 1.0f, 0.0f, 0.0f);
+    glRotatef(-roty+90.0, 0.0f, 1.0f, 0.0f);
+    glRotatef(-rotz, 0.0f, 0.0f, 1.0f);
+
+    glScalef(1.0, 1.0, -1.0);
+//    printf("rot: %f %f %f  pos: %f %f %f\n", rotx, roty, rotz, posx, posy, posz);
+
+	//-----------------    
+	// Draw the grid
+	//-----------------
+//    glPointSize(1.0);
+//    glColor3f(0.5, 0.5, 0.5);
+//    DrawGrid(-GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, 
+//	GRID_MINOR_STEP, GRID_MINOR_STEP, GRID_MINOR_STEP);
+
+
+	glLineWidth(1.0);
+	
+	glPointSize(4.0);
+	glColor3f(0.0, 0.0, 0.0);
+	DrawGrid(-GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, 
+		GRID_MAJOR_STEP, GRID_MAJOR_STEP, GRID_MAJOR_STEP);
+
+	//-----------------
+	// Plot the lighthouses
+	//-----------------
+	
+	glLineWidth(2.0);
+	
+	// Left lighthouse coordinate system
+	DrawCoordinateSystem(
+		houseL.x, houseL.y, houseL.z,
+		houseL.qi, houseL.qj, houseL.qk, houseL.qreal);
+	
+	// Rightt lighthouse coordinate system
+	DrawCoordinateSystem(
+		houseR.x, houseR.y, houseR.z,
+		houseR.qi, houseR.qj, houseR.qk, houseR.qreal);
+
+	glPointSize(15.0);
+	glBegin(GL_POINTS);
+	
+	// Left house in red
+	glColor3f(1,0,0);
+	glVertex3f(houseL.x, houseL.z, houseL.y);
+	
+	// Green is the origin (hmd)
+	glColor3f(0,1,0);
+	glVertex3f(0,0,0);
+	
+	// Right house in blue
+	glColor3f(0,0,1);
+	glVertex3f(houseR.x, houseR.z, houseR.y);
+	
+	glEnd();
+
+	//-------------------
+	// Draw the bearing angles
+	//-------------------
+	
+	// Read the hmd angles
+	OGLockMutex(read_mutex);
+	for (i=0; i<4; i++) {
+		for (j=0; j<32; j++) {
+			hmdAngles[i][j] = read_hmdAngles[i][j];
+		}
+	}
+	OGUnlockMutex(read_mutex);
+
+	// For every head mount angle
+	glBegin(GL_LINES);
+	for (i=0; i<32; i++) {
+	
+		// If the bearings exist
+		if (/*read_hmdAngleViewed[0][i] >= read_frameno-6 &&
+			read_hmdAngleViewed[1][i] >= read_frameno-6 &&
+			read_hmdAngleViewed[2][i] >= read_frameno-6 &&
+			read_hmdAngleViewed[3][i] >= read_frameno-6 &&*/
+			hmdAngles[0][i]!=-9999.0 && hmdAngles[1][i]!=-9999.0 && hmdAngles[2][i]!=-9999.0 && hmdAngles[3][i]!=-9999.0)
+		{
+			// Get the hmd bearings
+			double Ldx,Ldy,Ldz,Rdx,Rdy,Rdz;
+			HmdDir(hmdAngles[SWEEP_LX][i], hmdAngles[SWEEP_LY][i], &Ldx, &Ldy, &Ldz);
+			HmdDir(hmdAngles[SWEEP_RX][i], hmdAngles[SWEEP_RY][i], &Rdx, &Rdy, &Rdz);
+		
+			// Rotate by the lighthouse coordinate systems
+			Quaternion L0,L,R0,R,Lq,Rq;
+			QuaternionSet(L0,Ldx,Ldy,Ldz,0.0);
+			QuaternionSet(R0,Rdx,Rdy,Rdz,0.0);
+			QuaternionSet(Lq,houseL.qi,houseL.qj,houseL.qk,houseL.qreal);
+			QuaternionSet(Rq,houseR.qi,houseR.qj,houseR.qk,houseR.qreal);
+			QuaternionRot(L, Lq, L0);   // L is world space bearing from Left lighthouse to hmd
+			QuaternionRot(R, Rq, R0);   // R is world space bearing from Right lighthouse to hmd
+		
+			// Plot the lines
+			const double dist=10.0;
+			glVertex3f(houseL.x, houseL.z, houseL.y);
+			glVertex3f(houseL.x+dist*L.i, houseL.z+dist*L.k, houseL.y+dist*L.j);
+			glVertex3f(houseR.x, houseR.z, houseR.y);
+			glVertex3f(houseR.x+dist*R.i, houseR.z+dist*R.k, houseR.y+dist*R.j);
+		}
+	}
+	glEnd();
+
+	read_frameno++;
+
+    //
+    // Draw the Terrain
+    //
+    glutSwapBuffers ( );         // Swap The Buffers To Not Be Left With A Clear Screen
+}
+
+/*
+ * resize() is called when we change the screen size
+ */
+void resize(int width, int height)		// Resize And Initialize The GL Window
+{
+    glViewport(0,0,width,height);						// Reset The Current Viewport
+    
+    glMatrixMode(GL_PROJECTION);						// Select The Projection Matrix
+    glLoadIdentity();									// Reset The Projection Matrix
+    
+    // Uncomment For a 3D perspective
+    gluPerspective(45.0f,(float)width/(float)height,0.1f,1000.0f);
+    // Uncomment for a 2D perspective
+    //glOrtho(0.0, WIDTH, 0.0, HEIGHT, -10.0, 10.0);
+    
+    glMatrixMode(GL_MODELVIEW);							// Select The Modelview Matrix
+    glLoadIdentity();									// Reset The Modelview Matrix
+}
+
+/*
+ * These functions are called whenever a key is pressed
+ */
+void keyboardDown ( unsigned char key, int x, int y )  // Create Keyboard Function
+{
+    keys[key] = 1;
+}
+void keyboardUp ( unsigned char key, int x, int y )
+{
+    keys[key] = 0;
+}
+
+void specialKeyDown ( int key, int x, int y )  // Create Special Function (required for arrow keys)
+{
+    if (key<256) {
+        sp_keys[key] = 1;
+    }
+}
+void specialKeyUp (int key, int x, int y)
+{
+    if (key<256) {
+        sp_keys[key] = 0;
+    }
+}
+
+int main ( int argc, char** argv )   // Create Main Function For Bringing It All Together
+{
+    glutInit            ( &argc, argv ); // Erm Just Write It =)
+    glutInitDisplayMode ( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); // Display Mode
+    glutInitWindowSize  ( WIDTH, HEIGHT ); // If glutFullScreen wasn't called this is the window size
+    glutCreateWindow    ( "OpenGL" ); // Window Title (argv[0] for current directory as title)
+    if (FULLSCREEN) {
+        glutFullScreen      ( );          // Put Into Full Screen
+    }
+    glutDisplayFunc     ( draw );  // Matching Earlier Functions To Their Counterparts
+    glutIdleFunc        ( draw );
+    glutReshapeFunc     ( resize );
+    glutKeyboardFunc    ( keyboardDown );
+    glutKeyboardUpFunc  ( keyboardUp );
+    glutSpecialFunc     ( specialKeyDown );
+    glutSpecialUpFunc   ( specialKeyUp );
+    init ();
+    glutMainLoop        ( );          // Initialize The Main Loop
+    return 0;
+}
+
diff --git a/dave/plot_lighthouse/quaternion.h b/dave/plot_lighthouse/quaternion.h
new file mode 100755
index 0000000..050e5b8
--- /dev/null
+++ b/dave/plot_lighthouse/quaternion.h
@@ -0,0 +1,138 @@
+//
+//  quaternion.h
+//  Game
+//
+//  Created by user on 9/8/16.
+//  Copyright © 2016 user. All rights reserved.
+//
+
+#ifndef quaternion_h
+#define quaternion_h
+
+typedef struct {
+    float i,j,k,r;
+} Quaternion;
+typedef struct {
+    float x,y,z,theta;
+} AxisAngle;
+
+typedef struct {
+    Quaternion rot;   // orientation
+    Quaternion pos;   // positon
+} QuaternionBone;
+
+#define printq2(name,quat) printf("%s i %f j %f k %f r %f\n", name, quat.i, quat.j, quat.k, quat.r)
+
+#define QuaternionNormalize(quat) { \
+    float inv_len= 1.0f / sqrt( quat.i*quat.i + quat.j*quat.j + quat.k*quat.k + quat.r*quat.r ); \
+    quat.i *= inv_len; \
+    quat.j *= inv_len; \
+    quat.k *= inv_len; }
+
+#define QuaternionSet(res,I,J,K,R) { res.i=I; res.j=J; res.k=K; res.r=R; }
+
+#define AxisAngleOfQuaternion(res,q) { \
+    float len = sqrt(q.i*q.i + q.j*q.j + q.k*q.k); \
+    if (len > 0.0001) { \
+        float inv_len = 1.0f / len;  \
+        res.x = q.i * inv_len; \
+        res.y = q.j * inv_len; \
+        res.z = q.k * inv_len; \
+    } else { \
+        res.x=1.0f;  res.y=0.0f; res.z=0.0f; \
+    } \
+    res.theta = 2.0 * acos( q.r ); }
+
+#define QuaternionOfAxisAngle(res,aa) QuaternionSetAxisAngle(res, (aa).x, (aa).y, (aa).z, (aa).theta)
+
+#define QuaternionSetAxisAngle(res,x,y,z,theta) { \
+    float inv_lenXYZ = 1.0f / sqrt( (x)*(x) + (y)*(y) + (z)*(z) );       \
+    float cos_half_theta = cos( 0.5f*(theta) );     \
+    float sin_half_theta = sin( 0.5f*(theta) );     \
+    res.i = (x) * sin_half_theta * inv_lenXYZ; \
+    res.j = (y) * sin_half_theta * inv_lenXYZ; \
+    res.k = (z) * sin_half_theta * inv_lenXYZ; \
+    res.r =       cos_half_theta; }
+
+#define QuaternionIdentity(q) { q.i=0.0f; q.j=0.0f; q.k=0.0f; q.r=1.0f; }
+
+#define QuaternionInv(res,a) { \
+    res.i = -(a).i; \
+    res.j = -(a).j; \
+    res.k = -(a).k; \
+    res.r =  (a).r; }
+
+#define QuaternionMult(res,a,b) { \
+    res.i =  (a).i*(b).r + (a).j*(b).k - (a).k*(b).j + (a).r*(b).i; \
+    res.j = -(a).i*(b).k + (a).j*(b).r + (a).k*(b).i + (a).r*(b).j; \
+    res.k =  (a).i*(b).j - (a).j*(b).i + (a).k*(b).r + (a).r*(b).k; \
+    res.r = -(a).i*(b).i - (a).j*(b).j - (a).k*(b).k + (a).r*(b).r; }
+
+#define QuaternionSub(res,a,b) { \
+    res.i = (a).i - (b).i; \
+    res.j = (a).j - (b).j; \
+    res.k = (a).k - (b).k; \
+    res.r = (a).r - (b).r; }
+
+#define QuaternionAdd(res,a,b) { \
+    res.i = (a).i + (b).i; \
+    res.j = (a).j + (b).j; \
+    res.k = (a).k + (b).k; \
+    res.r = (a).r + (b).r; }
+
+#define QuaternionRot(res,q,p) { \
+    Quaternion q_inv,left;      \
+    QuaternionInv(q_inv,q);     \
+    QuaternionMult(left,q,p);   \
+    QuaternionMult(res,left,q_inv); }
+
+#define QuaternionSlerp(res,delta,u,v) { \
+    Quaternion rot,u_inv;        \
+    float theta,inv_s,scale;     \
+    QuaternionInv(u_inv,u);      \
+    QuaternionMult(rot,v,u_inv); \
+    if (rot.r < 0.998 && rot.r > -0.998) { \
+        theta  = acos(rot.r);        \
+        inv_s  = 1.0f / sin(theta);  \
+        theta *= delta;              \
+        scale  = inv_s * sin(theta); \
+        rot.i *= scale;              \
+        rot.j *= scale;              \
+        rot.k *= scale;              \
+        rot.r  = cos(theta);         \
+        QuaternionMult(res,rot,u);   \
+    } else {     \
+        res = u; \
+    } }
+
+/*
+#define QuaternionSlerp(res,delta,u,v) { \
+    Quaternion rot,u_inv;        \
+    float theta,inv_s,scale;     \
+    QuaternionInv(u_inv,u);      \
+    QuaternionMult(rot,v,u_inv); \
+    if (rot.r < 0.998) { \
+        printq2("slerp v",v); \
+        theta  = acos(rot.r);        \
+        inv_s  = 1.0f / sin(theta);  \
+        printf("slerp theta %f inv_s %f\n", theta, inv_s); \
+        theta *= delta;              \
+        scale  = inv_s * sin(theta); \
+        rot.i *= scale;              \
+        rot.j *= scale;              \
+        rot.k *= scale;              \
+        rot.r  = cos(theta);         \
+        QuaternionMult(res,rot,u);   \
+    } else {     \
+        res = u; \
+    } \
+    printq2("slerp res", res); }
+*/
+
+#define QuaternionBoneTransform(out,in,bone) {       \
+    Quaternion subtracted,rotated;              \
+    QuaternionSub(subtracted,in,bone.pos);      \
+    QuaternionRot(rotated,bone.rot,subtracted); \
+    QuaternionAdd(out,rotated,bone.pos); }
+
+#endif /* quaternion_h */