Switch from pos,quat to pose. Also change initialization order.

12 files changed, 776 insertions, 34 deletions

diff --git a/Makefile b/Makefile
index 25d510f..b4e65dd 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-all : lib data_recorder test calibrate calibrate_client
+all : lib data_recorder test calibrate calibrate_client simple_pose_test
 
 CC:=gcc
 
@@ -26,7 +26,7 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGCocoaNSImageDriver.o
 else
 
 LDFLAGS:=$(LDFLAGS) -lX11 -lusb-1.0
-DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c
+DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c redist/CNFG3D.c
 GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o
 
 endif
@@ -41,9 +41,9 @@ LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_
 
 
 #If you want to use HIDAPI on Linux.
-CFLAGS:=$(CFLAGS) -DHIDAPI
-REDISTS:=$(REDISTS) redist/hid-linux.o
-LDFLAGS:=$(LDFLAGS) -ludev
+#CFLAGS:=$(CFLAGS) -DHIDAPI
+#REDISTS:=$(REDISTS) redist/hid-linux.o
+#LDFLAGS:=$(LDFLAGS) -ludev
 
 #Useful Preprocessor Directives:
 # -DUSE_DOUBLE = use double instead of float for most operations.
@@ -62,12 +62,15 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
 
 # unused: redist/crc32.c
 
-testCocoa : testCocoa.c $(DRAWFUNCTIONS)
+testCocoa : testCocoa.c
 	$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
 test : test.c ./lib/libsurvive.so redist/os_generic.o
 	$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
+simple_pose_test : simple_pose_test.c ./lib/libsurvive.so redist/os_generic.o $(DRAWFUNCTIONS)
+	$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
+
 data_recorder : data_recorder.c ./lib/libsurvive.so redist/os_generic.c $(DRAWFUNCTIONS)
 	$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
@@ -92,7 +95,7 @@ calibrate_tcc : $(LIBSURVIVE_C)
 	tcc -DRUNTIME_SYMNUM $(CFLAGS) -o $@ $^ $(LDFLAGS) calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) redist/symbol_enumerator.c
 
 clean :
-	rm -rf *.o src/*.o *~ src/*~ test data_recorder calibrate testCocoa lib/libsurvive.so redist/*.o redist/*~
+	rm -rf *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so redist/*.o redist/*~
 
 
 
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index d9b5f08..f441c89 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -201,7 +201,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
 void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
-void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose);
 
 
 ////////////////////// Survive Drivers ////////////////////////////
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index be1115b..ba0c8f1 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -49,7 +49,7 @@ typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
 typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
-typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
+typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *pose);
 
 typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount);
 
diff --git a/redist/CNFG3D.c b/redist/CNFG3D.c
new file mode 100644
index 0000000..b574400
--- /dev/null
+++ b/redist/CNFG3D.c
@@ -0,0 +1,456 @@
+//Copyright 2012-2017 <>< Charles Lohr
+//You may license this file under the MIT/x11, NewBSD, or any GPL license.
+//This is a series of tools useful for software rendering.
+//Use of this file with OpenGL is untested.
+
+#include "CNFG3D.h"
+#include <string.h>
+#include <stdio.h>
+
+#define m00 0
+#define m01 1
+#define m02 2
+#define m03 3
+#define m10 4
+#define m11 5
+#define m12 6
+#define m13 7
+#define m20 8
+#define m21 9
+#define m22 10
+#define m23 11
+#define m30 12
+#define m31 13
+#define m32 14
+#define m33 15
+
+
+void tdIdentity( float * f )
+{
+	f[m00] = 1; f[m01] = 0; f[m02] = 0; f[m03] = 0;
+	f[m10] = 0; f[m11] = 1; f[m12] = 0; f[m13] = 0;
+	f[m20] = 0; f[m21] = 0; f[m22] = 1; f[m23] = 0;
+	f[m30] = 0; f[m31] = 0; f[m32] = 0; f[m33] = 1;
+}
+
+void tdZero( float * f )
+{
+	f[m00] = 0; f[m01] = 0; f[m02] = 0; f[m03] = 0;
+	f[m10] = 0; f[m11] = 0; f[m12] = 0; f[m13] = 0;
+	f[m20] = 0; f[m21] = 0; f[m22] = 0; f[m23] = 0;
+	f[m30] = 0; f[m31] = 0; f[m32] = 0; f[m33] = 0;
+}
+
+void tdTranslate( float * f, float x, float y, float z )
+{
+	float ftmp[16];
+	tdIdentity(ftmp);
+	ftmp[m03] += x;
+	ftmp[m13] += y;
+	ftmp[m23] += z;
+	tdMultiply( f, ftmp, f );
+}
+
+void tdScale( float * f, float x, float y, float z )
+{
+	f[m00] *= x;
+	f[m01] *= x;
+	f[m02] *= x;
+	f[m03] *= x;
+
+	f[m10] *= y;
+	f[m11] *= y;
+	f[m12] *= y;
+	f[m13] *= y;
+
+	f[m20] *= z;
+	f[m21] *= z;
+	f[m22] *= z;
+	f[m23] *= z;
+}
+
+void tdRotateAA( float * f, float angle, float ix, float iy, float iz )
+{
+	float ftmp[16];
+
+	float c = tdCOS( angle*tdDEGRAD );
+	float s = tdSIN( angle*tdDEGRAD );
+	float absin = tdSQRT( ix*ix + iy*iy + iz*iz );
+	float x = ix/absin;
+	float y = iy/absin;
+	float z = iz/absin;
+
+	ftmp[m00] = x*x*(1-c)+c;
+	ftmp[m01] = x*y*(1-c)-z*s;
+	ftmp[m02] = x*z*(1-c)+y*s;
+	ftmp[m03] = 0;
+
+	ftmp[m10] = y*x*(1-c)+z*s;
+	ftmp[m11] = y*y*(1-c)+c;
+	ftmp[m12] = y*z*(1-c)-x*s;
+	ftmp[m13] = 0;
+
+	ftmp[m20] = x*z*(1-c)-y*s;
+	ftmp[m21] = y*z*(1-c)+x*s;
+	ftmp[m22] = z*z*(1-c)+c;
+	ftmp[m23] = 0;
+
+	ftmp[m30] = 0;
+	ftmp[m31] = 0;
+	ftmp[m32] = 0;
+	ftmp[m33] = 1;
+
+	tdMultiply( f, ftmp, f );
+}
+
+void tdRotateEA( float * f, float x, float y, float z )
+{
+	float ftmp[16];
+
+	//x,y,z must be negated for some reason
+	float X = -x*2*tdQ_PI/360; //Reduced calulation for speed
+	float Y = -y*2*tdQ_PI/360;
+	float Z = -z*2*tdQ_PI/360;
+	float cx = tdCOS(X);
+	float sx = tdSIN(X);
+	float cy = tdCOS(Y);
+	float sy = tdSIN(Y);
+	float cz = tdCOS(Z);
+	float sz = tdSIN(Z);
+
+	//Row major
+	//manually transposed
+	ftmp[m00] = cy*cz;
+	ftmp[m10] = (sx*sy*cz)-(cx*sz);
+	ftmp[m20] = (cx*sy*cz)+(sx*sz);
+	ftmp[m30] = 0;
+
+	ftmp[m01] = cy*sz;
+	ftmp[m11] = (sx*sy*sz)+(cx*cz);
+	ftmp[m21] = (cx*sy*sz)-(sx*cz);
+	ftmp[m31] = 0;
+
+	ftmp[m02] = -sy;
+	ftmp[m12] = sx*cy;
+	ftmp[m22] = cx*cy;
+	ftmp[m32] = 0;
+
+	ftmp[m03] = 0;
+	ftmp[m13] = 0;
+	ftmp[m23] = 0;
+	ftmp[m33] = 1;
+
+	tdMultiply( f, ftmp, f );
+}
+
+void tdMultiply( float * fin1, float * fin2, float * fout )
+{
+	float fotmp[16];
+	int i, j, k;
+/*
+	fotmp[m00] = fin1[m00] * fin2[m00] + fin1[m01] * fin2[m10] + fin1[m02] * fin2[m20] + fin1[m03] * fin2[m30];
+	fotmp[m01] = fin1[m00] * fin2[m01] + fin1[m01] * fin2[m11] + fin1[m02] * fin2[m21] + fin1[m03] * fin2[m31];
+	fotmp[m02] = fin1[m00] * fin2[m02] + fin1[m01] * fin2[m12] + fin1[m02] * fin2[m22] + fin1[m03] * fin2[m32];
+	fotmp[m03] = fin1[m00] * fin2[m03] + fin1[m01] * fin2[m13] + fin1[m02] * fin2[m23] + fin1[m03] * fin2[m33];
+
+	fotmp[m10] = fin1[m10] * fin2[m00] + fin1[m11] * fin2[m10] + fin1[m12] * fin2[m20] + fin1[m13] * fin2[m30];
+	fotmp[m11] = fin1[m10] * fin2[m01] + fin1[m11] * fin2[m11] + fin1[m12] * fin2[m21] + fin1[m13] * fin2[m31];
+	fotmp[m12] = fin1[m10] * fin2[m02] + fin1[m11] * fin2[m12] + fin1[m12] * fin2[m22] + fin1[m13] * fin2[m32];
+	fotmp[m13] = fin1[m10] * fin2[m03] + fin1[m11] * fin2[m13] + fin1[m12] * fin2[m23] + fin1[m13] * fin2[m33];
+
+	fotmp[m20] = fin1[m20] * fin2[m00] + fin1[m21] * fin2[m10] + fin1[m22] * fin2[m20] + fin1[m23] * fin2[m30];
+	fotmp[m21] = fin1[m20] * fin2[m01] + fin1[m21] * fin2[m11] + fin1[m22] * fin2[m21] + fin1[m23] * fin2[m31];
+	fotmp[m22] = fin1[m20] * fin2[m02] + fin1[m21] * fin2[m12] + fin1[m22] * fin2[m22] + fin1[m23] * fin2[m32];
+	fotmp[m23] = fin1[m20] * fin2[m03] + fin1[m21] * fin2[m13] + fin1[m22] * fin2[m23] + fin1[m23] * fin2[m33];
+
+	fotmp[m30] = fin1[m30] * fin2[m00] + fin1[m31] * fin2[m10] + fin1[m32] * fin2[m20] + fin1[m33] * fin2[m30];
+	fotmp[m31] = fin1[m30] * fin2[m01] + fin1[m31] * fin2[m11] + fin1[m32] * fin2[m21] + fin1[m33] * fin2[m31];
+	fotmp[m32] = fin1[m30] * fin2[m02] + fin1[m31] * fin2[m12] + fin1[m32] * fin2[m22] + fin1[m33] * fin2[m32];
+	fotmp[m33] = fin1[m30] * fin2[m03] + fin1[m31] * fin2[m13] + fin1[m32] * fin2[m23] + fin1[m33] * fin2[m33];
+*/
+
+	for( i = 0; i < 16; i++ )
+	{
+		int xp = i & 0x03;
+		int yp = i & 0x0c;
+		fotmp[i] = 0;
+		for( k = 0; k < 4; k++ )
+		{
+			fotmp[i] += fin1[yp+k] * fin2[(k<<2)|xp];
+		}
+	}
+
+	tdMATCOPY( fout, fotmp );
+}
+
+void tdPrint( float * f )
+{
+	int i;
+	printf( "{\n" );
+	for( i = 0; i < 16; i+=4 )
+	{
+		printf( "  %f, %f, %f, %f\n", f[0+i], f[1+i], f[2+i], f[3+i] );
+	}
+	printf( "}\n" );
+}
+
+void tdTransposeSelf( float * f )
+{
+	float fout[16];
+	fout[m00] = f[m00]; fout[m01] = f[m10]; fout[m02] = f[m20]; fout[m03] = f[m30];
+	fout[m10] = f[m01]; fout[m11] = f[m11]; fout[m12] = f[m21]; fout[m13] = f[m31];
+	fout[m20] = f[m02]; fout[m21] = f[m12]; fout[m22] = f[m22]; fout[m23] = f[m32];
+	fout[m30] = f[m03]; fout[m31] = f[m13]; fout[m32] = f[m23]; fout[m33] = f[m33];
+	tdMATCOPY( f, fout );
+}
+
+
+void tdPerspective( float fovy, float aspect, float zNear, float zFar, float * out )
+{
+	float f = 1./tdTAN(fovy * tdQ_PI / 360.0);
+	out[m00] = f/aspect; out[m01] = 0; out[m02] = 0; out[m03] = 0;
+	out[m10] = 0; out[m11] = f; out[m12] = 0; out[m13] = 0;
+	out[m20] = 0; out[m21] = 0;
+	out[m22] = (zFar + zNear)/(zNear - zFar);
+	out[m23] = 2*zFar*zNear  /(zNear - zFar);
+	out[m30] = 0; out[m31] = 0; out[m32] = -1; out[m33] = 0;
+}
+
+void tdLookAt( float * m, float * eye, float * at, float * up )
+{
+	float out[16];
+	float otmp[16];
+	float F[3] = { at[0] - eye[0], at[1] - eye[1], at[2] - eye[2] };
+	float fdiv = 1./tdSQRT( F[0]*F[0] + F[1]*F[1] + F[2]*F[2] );
+	float f[3] = { F[0]*fdiv, F[1]*fdiv, F[2]*fdiv };
+	float udiv = 1./tdSQRT( up[0]*up[0] + up[1]*up[1] + up[2]*up[2] );
+	float UP[3] = { up[0]*udiv, up[1]*udiv, up[2]*udiv };
+	float s[3];
+	float u[3];
+	tdCross( f, UP, s );
+	tdCross( s, f, u );
+
+	out[m00] = s[0]; out[m01] = s[1]; out[m02] = s[2]; out[m03] = 0;
+	out[m10] = u[0]; out[m11] = u[1]; out[m12] = u[2]; out[m13] = 0;
+	out[m20] = -f[0];out[m21] =-f[1]; out[m22] =-f[2]; out[m23] = 0;
+	out[m30] = 0;    out[m31] = 0;    out[m32] = 0;    out[m33] = 1;
+
+	tdMultiply( m, out, m );
+	tdTranslate( m, -eye[0], -eye[1], -eye[2] );
+}
+
+
+
+
+
+
+
+
+
+
+
+
+void tdPTransform( const float * pin, float * f, float * pout )
+{
+	float ptmp[2];
+	ptmp[0] = pin[0] * f[m00] + pin[1] * f[m01] + pin[2] * f[m02] + f[m03];
+	ptmp[1] = pin[0] * f[m10] + pin[1] * f[m11] + pin[2] * f[m12] + f[m13];
+	pout[2] = pin[0] * f[m20] + pin[1] * f[m21] + pin[2] * f[m22] + f[m23];
+	pout[0] = ptmp[0];
+	pout[1] = ptmp[1];
+}
+
+void tdVTransform( const float * pin, float * f, float * pout )
+{
+	float ptmp[2];
+	ptmp[0] = pin[0] * f[m00] + pin[1] * f[m01] + pin[2] * f[m02];
+	ptmp[1] = pin[0] * f[m10] + pin[1] * f[m11] + pin[2] * f[m12];
+	pout[2] = pin[0] * f[m20] + pin[1] * f[m21] + pin[2] * f[m22];
+	pout[0] = ptmp[0];
+	pout[1] = ptmp[1];
+}
+
+void td4Transform( float * pin, float * f, float * pout )
+{
+	float ptmp[3];
+	ptmp[0] = pin[0] * f[m00] + pin[1] * f[m01] + pin[2] * f[m02] + pin[3] * f[m03];
+	ptmp[1] = pin[0] * f[m10] + pin[1] * f[m11] + pin[2] * f[m12] + pin[3] * f[m13];
+	ptmp[2] = pin[0] * f[m20] + pin[1] * f[m21] + pin[2] * f[m22] + pin[3] * f[m23];
+	pout[3] = pin[0] * f[m30] + pin[1] * f[m31] + pin[2] * f[m32] + pin[3] * f[m33];
+	pout[0] = ptmp[0];
+	pout[1] = ptmp[1];
+	pout[2] = ptmp[2];
+}
+
+void td4RTransform( float * pin, float * f, float * pout )
+{
+	float ptmp[3];
+	ptmp[0] = pin[0] * f[m00] + pin[1] * f[m10] + pin[2] * f[m20] + pin[3] * f[m30];
+	ptmp[1] = pin[0] * f[m01] + pin[1] * f[m11] + pin[2] * f[m21] + pin[3] * f[m31];
+	ptmp[2] = pin[0] * f[m02] + pin[1] * f[m12] + pin[2] * f[m22] + pin[3] * f[m32];
+	pout[3] = pin[0] * f[m03] + pin[1] * f[m13] + pin[2] * f[m23] + pin[3] * f[m33];
+	pout[0] = ptmp[0];
+	pout[1] = ptmp[1];
+	pout[2] = ptmp[2];
+}
+
+void tdNormalizeSelf( float * vin )
+{
+	float vsq = 1./tdSQRT(vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2]);
+	vin[0] *= vsq;
+	vin[1] *= vsq;
+	vin[2] *= vsq;
+}
+
+void tdCross( float * va, float * vb, float * vout )
+{
+	float vtmp[2];
+	vtmp[0] = va[1] * vb[2] - va[2] * vb[1];
+	vtmp[1] = va[2] * vb[0] - va[0] * vb[2];
+	vout[2] = va[0] * vb[1] - va[1] * vb[0];
+	vout[0] = vtmp[0];
+	vout[1] = vtmp[1];
+}
+
+float tdDistance( float * va, float * vb )
+{
+	float dx = va[0]-vb[0];
+	float dy = va[1]-vb[1];
+	float dz = va[2]-vb[2];
+
+	return tdSQRT(dx*dx + dy*dy + dz*dz);
+}
+
+float tdDot( float * va, float * vb )
+{
+	return va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2];
+}
+
+//Stack functionality.
+static float gsMatricies[2][tdMATRIXMAXDEPTH];
+float * gSMatrix = &gsMatricies[0][0];
+static int gsMMode;
+static int gsMPlace[2];
+
+void tdPush()
+{
+	if( gsMPlace[gsMMode] > tdMATRIXMAXDEPTH - 2 )
+		return;
+
+	tdMATCOPY( &gsMatricies[gsMMode][gsMPlace[gsMMode]], &gsMatricies[gsMMode][gsMPlace[gsMMode] + 1] );
+	gsMPlace[gsMMode]++;
+
+	gSMatrix = &gsMatricies[gsMMode][gsMPlace[gsMMode]];
+}
+
+void tdPop()
+{
+	if( gsMPlace[gsMMode] < 1 )
+		return;
+
+	gsMPlace[gsMMode]--;
+
+	gSMatrix = &gsMatricies[gsMMode][gsMPlace[gsMMode]];
+
+}
+
+void tdMode( int mode )
+{
+	if( mode < 0 || mode > 1 )
+		return;
+	
+	gsMMode = mode;
+
+	gSMatrix = &gsMatricies[gsMMode][gsMPlace[gsMMode]];
+
+}
+
+static float translateX;
+static float translateY;
+static float scaleX;
+static float scaleY;
+
+void tdSetViewport( float leftx, float topy, float rightx, float bottomy, float pixx, float pixy )
+{
+	translateX = leftx;
+	translateY = bottomy;
+	scaleX = pixx/(rightx-leftx);
+	scaleY = pixy/(topy-bottomy);
+
+}
+
+void tdFinalPoint( float * pin, float * pout )
+{
+	float tdin[4] = { pin[0], pin[1], pin[2], 1. };
+	float tmp[4];
+	td4Transform( tdin, &gsMatricies[0][gsMPlace[0]], tmp );
+//	printf( "XFORM1Out: %f %f %f %f\n", tmp[0], tmp[1], tmp[2], tmp[3] );
+	td4Transform(  tmp, &gsMatricies[1][gsMPlace[1]], tmp );
+//	printf( "XFORM2Out: %f %f %f %f\n", tmp[0], tmp[1], tmp[2], tmp[3] );
+	pout[0] = (tmp[0]/tmp[3] - translateX) * scaleX;
+	pout[1] = (tmp[1]/tmp[3] - translateY) * scaleY;
+	pout[2] = tmp[2]/tmp[3];
+//	printf( "XFORMFOut: %f %f %f\n", pout[0], pout[1], pout[2] );
+}
+
+
+
+
+
+
+
+
+
+
+static inline float tdNoiseAt( int x, int y )
+{
+	return ((x*13241 + y * 33455927)%9293) / 9292.;
+//	srand( x + y * 1314);
+//	return ((rand()%1000)/500.) - 1.0;
+}
+
+static inline float tdFade( float f )
+{
+	float ft3 = f*f*f;
+	return ft3 * 10 - ft3 * f * 15 + 6 * ft3 * f * f;
+}
+
+float tdFLerp( float a, float b, float t )
+{
+	float fr = tdFade( t );
+	return a * (1.-fr) + b * fr;
+}
+
+static inline float tdFNoiseAt( float x, float y )
+{
+	int ix = x;
+	int iy = y;
+	float fx = x - ix;
+	float fy = y - iy;
+
+	float a = tdNoiseAt( ix, iy );
+	float b = tdNoiseAt( ix+1, iy );
+	float c = tdNoiseAt( ix, iy+1 );
+	float d = tdNoiseAt( ix+1, iy+1 );
+
+	float top = tdFLerp( a, b, fx );
+	float bottom = tdFLerp( c, d, fx );
+
+	return tdFLerp( top, bottom, fy );
+}
+
+float tdPerlin2D( float x, float y )
+{
+	int ndepth = 5;
+
+	int depth;
+	float ret = 0;
+	for( depth = 0; depth < ndepth; depth++ )
+	{
+		float nx = x / (1<<(ndepth-depth-1));
+		float ny = y / (1<<(ndepth-depth-1));
+		ret += tdFNoiseAt( nx, ny ) / (1<<(depth+1));
+	}
+	return ret;
+}
+
diff --git a/redist/CNFG3D.h b/redist/CNFG3D.h
new file mode 100644
index 0000000..7f74d96
--- /dev/null
+++ b/redist/CNFG3D.h
@@ -0,0 +1,66 @@
+//Copyright 2012-2017 <>< Charles Lohr
+//You may license this file under the MIT/x11, NewBSD, or any GPL license.
+
+//This is a series of tools useful for software rendering.
+//Use of this file with OpenGL is untested.
+
+#ifndef _3D_H
+#define _3D_H
+
+#include <math.h>
+#define tdCOS cosf
+#define tdSIN sinf
+#define tdTAN tanf
+#define tdSQRT sqrtf
+#define tdMATCOPY(x,y) memcpy( x, y, 16*sizeof(float))
+#define tdQ_PI 3.141592653589
+#define tdDEGRAD (tdQ_PI/180.)
+#define tdRADDEG (180./tdQ_PI)
+
+
+//General Matrix Functions
+void tdIdentity( float * f );
+void tdZero( float * f );
+void tdTranslate( float * f, float x, float y, float z );		//Operates ON f
+void tdScale( float * f, float x, float y, float z );			//Operates ON f
+void tdRotateAA( float * f, float angle, float x, float y, float z ); 	//Operates ON f
+void tdRotateEA( float * f, float x, float y, float z );		//Operates ON f
+void tdMultiply( float * fin1, float * fin2, float * fout );		//Operates ON f
+void tdPrint( float * f );
+void tdTransposeSelf( float * f );
+
+//Specialty Matrix Functions
+void tdPerspective( float fovy, float aspect, float zNear, float zFar, float * out ); //Sets, NOT OPERATES. (FOVX=degrees)
+void tdLookAt( float * m, float * eye, float * at, float * up );	//Operates ON f
+
+//General point functions
+#define tdPSet( f, x, y, z ) { f[0] = x; f[1] = y; f[2] = z; }
+void tdPTransform( const float * pin, float * f, float * pout );
+void tdVTransform( const float * vin, float * f, float * vout );
+void td4Transform( float * kin, float * f, float * kout );
+void td4RTransform( float * kin, float * f, float * kout );
+void tdNormalizeSelf( float * vin );
+void tdCross( float * va, float * vb, float * vout );
+float tdDistance( float * va, float * vb );
+float tdDot( float * va, float * vb );
+#define tdPSub( x, y, z ) { (z)[0] = (x)[0] - (y)[0]; (z)[1] = (x)[1] - (y)[1]; (z)[2] = (x)[2] - (y)[2]; }
+#define tdPAdd( x, y, z ) { (z)[0] = (x)[0] + (y)[0]; (z)[1] = (x)[1] + (y)[1]; (z)[2] = (x)[2] + (y)[2]; }
+
+//Stack Functionality
+#define tdMATRIXMAXDEPTH 32
+extern float * gSMatrix;
+void tdPush();
+void tdPop();
+void tdMode( int mode );
+#define tdMODELVIEW 0
+#define tdPROJECTION 1
+
+//Final stage tools
+void tdSetViewport( float leftx, float topy, float rightx, float bottomy, float pixx, float pixy );
+void tdFinalPoint( float * pin, float * pout );
+
+
+float tdFLerp( float a, float b, float t );
+float tdPerlin2D( float x, float y );
+
+#endif
diff --git a/redist/linmath.c b/redist/linmath.c
index 1d70ee1..4240ed6 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -490,6 +490,8 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
 	vquat[2] = vec3in[1];
 	vquat[3] = vec3in[2];
 
+	//XXX WARNING: This code is probably SLOW.  See this:  https://github.com/axlecrusher/hgengine3/blob/master/Mercury3/basic_light1_v.glsl
+
 	quatrotateabout( tquat, quat, vquat );
 	quatgetconjugate( qrecp, quat );
 	quatrotateabout( vquat, tquat, qrecp );
@@ -637,3 +639,11 @@ void matrix44transpose(FLT * mout, const FLT * minm )
 
 }
 
+void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose )
+{
+	FLT v3o[3];
+	quatrotatevector( v3o, &pose[3], pin );
+	for(int i = 0; i < 3; i++)
+		pout[i] = pose[i] + v3o[i];
+}
+
diff --git a/simple_pose_test.c b/simple_pose_test.c
new file mode 100644
index 0000000..4b392b9
--- /dev/null
+++ b/simple_pose_test.c
@@ -0,0 +1,197 @@
+#ifdef __linux__
+#include <unistd.h>
+#endif
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <survive.h>
+#include "src/survive_cal.h"
+#include <CNFG3D.h>
+#include <os_generic.h>
+#include <CNFGFunctions.h>
+#include <string.h>
+#include <linmath.h>
+struct SurviveContext * ctx;
+
+void HandleKey( int keycode, int bDown )
+{
+	if( !bDown ) return;
+
+	if( keycode == 'O' || keycode == 'o' )
+	{
+		survive_send_magic(ctx,1,0,0);
+	}
+	if( keycode == 'F' || keycode == 'f' )
+	{
+		survive_send_magic(ctx,0,0,0);
+	}
+}
+
+void HandleButton( int x, int y, int button, int bDown )
+{
+}
+
+void HandleMotion( int x, int y, int mask )
+{
+}
+
+void HandleDestroy()
+{
+}
+
+FLT hpos[3];
+FLT hpos2[3];
+
+void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose)
+{
+	survive_default_raw_pose_process(so, lighthouse, pose );
+
+	if( lighthouse != 0 || strcmp( so->codename, "HMD" ) != 0 )
+		return;
+
+	// print the pose;
+/*	double qw = quat[0];
+	double qx = quat[1];
+	double qy = quat[2];
+	double qz = quat[3];
+
+	hra = 2 * acos(qw);
+	hrx = qx / sqrt(1-qw*qw);
+	hry = qy / sqrt(1-qw*qw);
+	hrz = qz / sqrt(1-qw*qw);
+	hra *=  180/3.14159;
+
+	hx = pos[0];
+	hy = pos[1];
+	hz = pos[2];*/
+
+	printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6] );
+
+	hpos[0] = pose[0];
+	hpos[1] = pose[1];
+	hpos[2] = pose[2];
+	FLT hposin[3] = { 0, 0, 1 };
+	ApplyPoseToPoint( hpos2, hposin, pose );
+
+	fflush(stdout);
+}
+
+
+void testprog_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
+{
+//	printf( "%d: %d %d\n", sensor_id, acode, timecode );
+}
+
+void DrawLineSegment( float x, float y, float z, float x2, float y2, float z2 )
+{
+	//float pti[6] = { 1, sin(TimeSinceStart), cos(TimeSinceStart ), 1, 0, 0};
+	float pa[3] = { x, y, z };
+	float pb[3] = { x2, y2, z2 };
+
+	tdFinalPoint( pa, pa );
+	tdFinalPoint( pb, pb );
+
+	if( pa[2] >= 1.0 ) return;
+	if( pb[2] >= 1.0 ) return;
+	if( pa[2] < 0 ) return;
+	if( pb[2] < 0 ) return;
+
+	int dx, dy;
+	for( dx = 0; dx < 2; dx++ )
+	for( dy = 0; dy < 2; dy++ )
+	{
+		CNFGTackPixel( pa[0]+dx, pa[1]+dy );
+		CNFGTackSegment( pa[0]+dx, pa[1]+dy, pb[0]+dx, pb[1]+dy );
+	}
+}
+
+
+void *GUIThread(void*v)
+{
+	CNFGSetup( "Orientation test", 640, 480 );
+	double fLast = 0;
+	short screenx, screeny;
+	double TimeSinceStart = 0;
+	while(1)
+	{
+		double Now = OGGetAbsoluteTime();
+		double dt = Now - fLast;
+		fLast = Now;
+		TimeSinceStart += dt;
+
+		CNFGClearFrame();
+		CNFGHandleInput();
+
+		CNFGColor( 0xFFFFFF );
+		CNFGGetDimensions( &screenx, &screeny );
+
+		int x, y;
+		float eye[3] = { 8, 8, 8};
+		float at[3] = { 0,0, 0 };
+		float up[3] = { 0,0, 1 };
+
+		tdSetViewport( -1, -1, 1, 1, screenx, screeny );
+
+		tdMode( tdPROJECTION );
+		tdIdentity( gSMatrix );
+		tdPerspective( 40, ((float)screenx)/((float)screeny), .1, 200., gSMatrix );
+
+		tdMode( tdMODELVIEW );
+		tdIdentity( gSMatrix );
+		tdLookAt( gSMatrix, eye, at, up );
+
+		CNFGColor( 0x00ffff ); DrawLineSegment( hpos[0], hpos[1], hpos[2], hpos2[0], hpos2[1], hpos2[2] );
+		CNFGColor( 0x0000ff ); DrawLineSegment( 0, 0, 0, 1, 0, 0  );
+		CNFGColor( 0xff0000 ); DrawLineSegment( 0, 0, 0, 0, 1, 0  );
+		CNFGColor( 0x00ff00 ); DrawLineSegment( 0, 0, 0, 0, 0, 1  );
+		CNFGColor( 0xffffff ); DrawLineSegment( 0, 0, 0, 0, 0, 0  );
+		CNFGSwapBuffers();
+		OGUSleep(10000);
+	}
+}
+
+
+int main()
+{
+	int magicon = 0;
+	double Start = OGGetAbsoluteTime();
+
+	ctx = survive_init( 0 );
+
+	//survive_install_button_fn(ctx, testprog_button_process);
+	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+	//survive_install_imu_fn(ctx, testprog_imu_process);
+	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+	//survive_install_angle_fn(ctx, testprog_angle_process );
+
+	ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1;
+	int i;
+	for( i = 0; i < 2; i++ )
+	{
+		SurvivePose * p = &ctx->bsd[i].Pose;
+		p->Pos[0] = 0;
+		p->Pos[1] = 0;
+		p->Pos[2] = 0;
+		p->Rot[0] = 1;
+		p->Rot[1] = 0;
+		p->Rot[2] = 0;
+		p->Rot[3] = 0;
+	}
+
+	OGCreateThread( GUIThread, 0 );
+
+	if( !ctx )
+	{
+		fprintf( stderr, "Fatal. Could not start\n" );
+		return 1;
+	}
+
+	//survive_cal_install(ctx);
+	
+
+	while(survive_poll(ctx) == 0)
+	{
+		//Do Stuff
+	}
+}
+
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 94d572e..4398b66 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1616,7 +1616,7 @@ static void QuickPose(SurviveObject *so, int lh)
 
 		if (sensorCount > 4)
 		{
-			FLT pos[3], quat[4];
+			FLT pose[7];
 
 			// TODO: This countdown stuff is a total hack!
 			// it basically ignores all the logic to find the most reliable data points
@@ -1637,12 +1637,12 @@ static void QuickPose(SurviveObject *so, int lh)
 
 
 
-			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+			SolveForLighthouse(&pose[0], &pose[3], to, so, 0, lh, 0);
 
 			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
 			if (so->ctx->rawposeproc)
 			{
-				so->ctx->rawposeproc(so, lh, pos, quat);
+				so->ctx->rawposeproc(so, lh, pose);
 			}
 
 			if (ttDebug) printf("!\n");
diff --git a/src/survive.c b/src/survive.c
index 76bf8e4..21e1f0e 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -138,7 +138,6 @@ SurviveContext * survive_init_internal( int headless )
 	init_config_group(&ctx->lh_config[1],10);
 
 	config_read(ctx, "config.json");
-
 	ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2);
 	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0);
 	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1);
@@ -150,16 +149,19 @@ SurviveContext * survive_init_internal( int headless )
 	ctx->imuproc = survive_default_imu_process;
 	ctx->angleproc = survive_default_angle_process;
 
-	const char * DriverName;
-	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
-	{
-		DeviceDriver dd = GetDriver( DriverName );
-		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
-		r = dd( ctx );
-		printf( "Driver %s reports status %d\n", DriverName, r );
-	}
+
+	// initialize the button queue
+	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
+	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
+
+	// start the thread to process button data
+	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
+	survive_install_button_fn(ctx, NULL);
+	survive_install_raw_pose_fn(ctx, NULL);
 
 	i = 0;
+	const char * DriverName;
+
 	//const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy");
 	const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
@@ -179,6 +181,17 @@ SurviveContext * survive_init_internal( int headless )
 		SV_ERROR( "Error.  Cannot find any valid poser." );
 	}
 
+	i = 0;
+	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
+	{
+		DeviceDriver dd = GetDriver( DriverName );
+		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
+		r = dd( ctx );
+		printf( "Driver %s reports status %d\n", DriverName, r );
+	}
+printf( "REGISTERING DRIVERS\n" );
+
+	//Apply poser to objects.
 	for( i = 0; i < ctx->objs_ct; i++ )
 	{
 		ctx->objs[i]->PoserFn = PreferredPoserCB;
@@ -187,15 +200,6 @@ SurviveContext * survive_init_internal( int headless )
 	// saving the config extra to make sure that the user has a config file they can change.
 	config_save(ctx, "config.json");
 
-	// initialize the button queue
-	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
-
-	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
-
-	// start the thread to process button data
-	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
-	survive_install_button_fn(ctx, NULL);
-	survive_install_raw_pose_fn(ctx, NULL);
 
 	return ctx;
 }
diff --git a/src/survive_process.c b/src/survive_process.c
index 0f19007..1ea3061 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -59,6 +59,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 			.angle = angle,
 			.lh = lh,
 		};
+
 		so->PoserFn( so, (PoserData *)&l );
 	}
 }	
@@ -101,7 +102,7 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 	//}
 }
 
-void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose)
 {
 	// print the pose;
 	//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
diff --git a/src/survive_vive.c b/src/survive_vive.c
index b3d990a..5001efd 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1655,6 +1655,11 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int
 	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
 	printf( "Loading config: %d\n", len );
 
+	if( len < 0 )
+	{
+		return len;
+	}
+
 	{
 	  char raw_fname[100];
 	  sprintf( raw_fname, "%s_config.json", so->codename );
diff --git a/test.c b/test.c
index 5fc62af..ef7ae45 100644
--- a/test.c
+++ b/test.c
@@ -84,12 +84,12 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt
 	}
 }
 
-void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose)
 {
-	survive_default_raw_pose_process(so, lighthouse, pos, quat);
+	survive_default_raw_pose_process(so, lighthouse, pose);
 
 	// print the pose;
-	printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+	printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]);
 }
 
 void testprog_imu_process(SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id)