Added a dynamic plotting tool for OrthoSolve. Charles added orthogonalization to the rotation matrix.

7 files changed, 988 insertions, 312 deletions

diff --git a/dave/AffineSolve b/dave/AffineSolve
index 98a9590..7d51b34 100755
--- a/dave/AffineSolve
+++ b/dave/AffineSolve
diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 4fba56b..685062e 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -11,10 +11,13 @@
 #include <math.h>
 #include "dclapack.h"
 #include <linmath.h>
+#define RegisterDriver(a,b)
+#include "poser_daveortho.c"
 #define LH_ID    0
 #define NUM_HMD 32
+#define INDIR "full_test_triangle_on_floor/"
 
-#define MAX_POINTS 128
+#define MAX_POINTS SENSORS_PER_OBJECT
 //#define _ABS(a)  ( (a)<=0 ? -(a) : (a) )
 #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f )
 #define RANDF  ( (float)rand() / (float)RAND_MAX )
@@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3];
 void ReadHmdPoints()
 {
     int i;
-    FILE *fin = fopen("HMD_points.csv","r");
+    FILE *fin = fopen(INDIR "HMD_points.csv","r");
     if (fin==NULL) {
         printf("ERROR: could not open HMD_points.csv for reading\n");
         exit(1);
@@ -52,7 +55,7 @@ void ReadPtinfo()
     for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; }
 
     // Read ptinfo.csv
-    FILE *fin = fopen("ptinfo.csv", "r");
+    FILE *fin = fopen(INDIR "ptinfo.csv", "r");
     if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
     while (!feof(fin))
     {
@@ -74,6 +77,350 @@ void ReadPtinfo()
     fclose(fin);
 }
 
+void AffineSolve(
+    float T[4][4],           // OUTPUT: transform
+    float O[MAX_POINTS][4],  // INPUT:  points, offsets
+    float N[MAX_POINTS][3],  // INPUT:  plane normals
+    float D[MAX_POINTS],     // INPUT:  plane offsets
+    int nPoints, int nIter,
+    float stepSizeRot, float stepSizePos, float falloff, int constrain)
+{
+    int i,j,k,iter;
+    //T[3][3] = 1.0f;
+    
+    printf("iter x y z error\n");
+    
+    float gradDot     = 1.0;
+    float prevGradDot = 1.0;
+    float de_dT[3][4];  // the gradient
+    float conj[3][4];   // the conjugate
+    float errorSq=0.0;
+    for (iter=0; iter<nIter; iter++)
+    {
+        //----------------------------------
+        // Calculate the gradient direction
+        //----------------------------------
+        errorSq = 0.0;
+        memset(de_dT, 0, 3*4*sizeof(float));
+        for (i=0; i<nPoints; i++)
+        {
+            // What is the plane deviation error
+            float Ei = -D[i];
+            for (j=0; j<3; j++) {
+                float Tj_oi = 0.0f;
+                for (k=0; k<4; k++) {
+                    Tj_oi += T[j][k] * O[i][k];
+                }
+                Ei += N[i][j] * Tj_oi;
+            }
+//            printf("E[%d] %f\n", i, Ei);
+        
+            // Figure out contribution to the error
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    de_dT[j][k] += N[i][j] * O[i][k] * Ei;
+                }
+            }
+            
+            errorSq += Ei*Ei;
+        }
+
+//        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
+//exit(1);
+        // Constrain the gradient (such that dot products are zero)
+        if (constrain)
+        {
+            float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
+            for (k=0; k<3; k++) {
+                T0T1 += T[0][k] * T[1][k];
+                T1T2 += T[1][k] * T[2][k];
+                T2T0 += T[2][k] * T[0][k];
+            }
+//            printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
+            for (k=0; k<3; k++) {
+                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
+                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
+                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
+                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
+                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
+                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
+            }
+        }
+
+        // Calculate the gradient dot product
+        //  (used by conjugate gradient method)
+        prevGradDot = gradDot;
+        gradDot = 0.0;
+        for (j=0; j<3; j++) {
+            for (k=0; k<4; k++) {
+                gradDot += de_dT[j][k] * de_dT[j][k];
+            }
+        }
+
+//        printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
+
+        //----------------------------------
+        // Calculate the conjugate direction
+        //----------------------------------
+//        if (iter==0) {
+            // First iteration, just use the gradient
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    conj[j][k] = -de_dT[j][k];
+                }
+            }
+/*        } else {
+            // Calculate "beta" for Fletcher Reeves method
+            float beta = gradDot / prevGradDot;
+//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
+
+            // Update the conjugate
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    conj[j][k] = beta*conj[j][k] - de_dT[j][k];
+                }
+            }            
+        }
+*/
+
+//        PRINT_MAT(de_dT,4,4);
+//        exit(1);
+
+        //----------------------------------
+        // How large is the gradient ?
+        //----------------------------------
+        
+        double gradSizeRot = 0.0;
+        double gradSizePos = 0.0;
+        for (j=0; j<3; j++) {
+            for (k=0; k<3; k++) {
+                gradSizeRot += _ABS(conj[j][k]);
+            }
+            gradSizePos += _ABS(conj[j][k]);
+        }
+        if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; }  // Quit, we've totally converged
+        
+        //----------------------------------
+        // Descend in the gradient direction
+        //----------------------------------
+        if (gradSizeRot > TOO_SMALL) {
+            float scaleRot = stepSizeRot / gradSizeRot;
+            for (j=0; j<3; j++) {
+                for (k=0; k<3; k++) {
+                    T[j][k] += scaleRot * conj[j][k];
+                }
+            }
+            stepSizeRot *= falloff;
+        }
+        
+        if (gradSizePos > TOO_SMALL) {
+            float scalePos = stepSizePos / gradSizePos;
+            for (j=0; j<3; j++) {
+                T[j][3] += scalePos * conj[j][3];
+            }
+            stepSizePos *= falloff;
+        }
+        
+        // Constrain the gradient (such that scaling is one)
+        if (constrain)
+        {
+            // Measure the scales
+            float len[3] = {0.0, 0.0, 0.0};
+            for (j=0; j<3; j++) {
+                double lenSq = 0.0;
+                for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
+                len[j] = sqrt(lenSq);
+            }
+            
+            // How far off is the scale?
+            float xzLen = 0.5 * (len[0] + len[2]);
+            if (xzLen > TOO_SMALL) {
+                float inv_xzLen = 1.0 / xzLen;
+                for (j=0; j<3; j++) {
+                    T[3][j] *= inv_xzLen;
+                }
+            }
+            
+            // Rescale the thing
+            for (j=0; j<3; j++)
+            {
+                if (len[j] > TOO_SMALL) {
+                    float inv_len = 1.0 / len[j];
+                    for (k=0; k<3; k++) { T[j][k] *= inv_len; }
+                }
+            }
+        }
+    }
+    float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]);
+    printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist);
+}
+
+int main()
+{
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    ReadHmdPoints();
+    ReadPtinfo();
+
+    //-------------------------
+    // Package the lighthouse data for "AffineSolve"
+    //-------------------------
+
+    // Data for the "iterative" affine solve formula
+    float Tcalc[4][4];
+    float O[MAX_POINTS][4];
+    float N[MAX_POINTS][3];
+    float D[MAX_POINTS];
+    int nPlanes = 0;
+
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        for (axis=0; axis<2; axis++)
+        {
+            if (hmd_angle[sen][axis] != -9999.0)
+            {
+                // Set the offset
+                O[nPlanes][0] = hmd_pos[sen][0];
+                O[nPlanes][1] = hmd_pos[sen][1];
+                O[nPlanes][2] = hmd_pos[sen][2];
+                O[nPlanes][3] = 1.0;
+        
+                // Calculate the plane equation
+                if (axis == 0) {   // Horizontal
+                    N[nPlanes][0] = -cos(hmd_angle[sen][axis]);
+                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
+                    N[nPlanes][2] = 0.0;
+                    D[nPlanes]    = 0.0;
+                } else {           // Vertical
+                    N[nPlanes][0] = 0.0;
+                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
+                    N[nPlanes][2] =  cos(hmd_angle[sen][axis]);
+                    D[nPlanes]    = 0.0;
+                }
+
+                printf("plane %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
+                    nPlanes,
+                    O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], 
+                    N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], 
+                    D[nPlanes]);
+                nPlanes++;
+            }
+        }
+    }
+
+    
+    printf("nPlanes %d\n", nPlanes);
+        
+    //}
+    
+    PRINT_MAT(Tcalc,4,4);
+
+    
+    //--------------------------------------------------
+    // Package the data for "OrthoSolve"
+    //--------------------------------------------------
+
+    // Data for the "fake" ortho solve formula
+    float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
+    FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
+    FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
+    FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
+    int nPoints=0;  
+
+    // Transform into the "OrthoSolve" format
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
+        {
+            S_in[0][nPoints] = hmd_angle[sen][0];
+            S_in[1][nPoints] = hmd_angle[sen][1];
+            X_in[0][nPoints] = hmd_pos[sen][0];
+            X_in[1][nPoints] = hmd_pos[sen][1];
+            X_in[2][nPoints] = hmd_pos[sen][2];
+            nPoints++;
+        }
+    }
+    printf("OrthoSolve nPoints %d\n", nPoints);
+    
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    int loop;
+    for (loop=0; loop<1; loop++)
+    {
+        // Run OrthoSolve
+        OrthoSolve(
+            Tortho,     // OUTPUT: 4x4 transformation matrix
+            S_out,      // OUTPUT: array of output screenspace points
+            S_in,       // INPUT:  array of screenspace points
+            X_in,       // INPUT:  array of offsets
+            nPoints);
+			printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+		// Come up with rotation and transposed version of Tortho
+		FLT TorthoTr[4][4], Rortho[4][4], RorthoTr[4][4];
+		TRANSP(Tortho,TorthoTr,4,4);
+		memcpy(Rortho,Tortho,4*4*sizeof(FLT));
+		Rortho[3][0]=0.0;  Rortho[3][1]=0.0;  Rortho[3][2]=0.0;
+		TRANSP(Rortho,RorthoTr,4,4);
+		PRINT(Tortho,4,4);
+		PRINT(Rortho,4,4);
+
+		// Print out some quaternions
+		FLT Tquat[4], TquatTr[4], Rquat[4], RquatTr[4];
+		quatfrommatrix(Tquat,   &Tortho[0][0] );
+		quatfrommatrix(TquatTr, &TorthoTr[0][0] );
+		quatfrommatrix(Rquat,   &Rortho[0][0] );
+		quatfrommatrix(RquatTr, &RorthoTr[0][0] );
+		printf( "Tquat  : %f %f %f %f = %f\n", Tquat  [0], Tquat  [1], Tquat  [2], Tquat  [3], quatmagnitude(Tquat));
+		printf( "TquatTr: %f %f %f %f = %f\n", TquatTr[0], TquatTr[1], TquatTr[2], TquatTr[3], quatmagnitude(TquatTr));
+		printf( "Rquat  : %f %f %f %f = %f\n", Rquat  [0], Rquat  [1], Rquat  [2], Rquat  [3], quatmagnitude(Rquat));
+		printf( "RquatTr: %f %f %f %f = %f\n", RquatTr[0], RquatTr[1], RquatTr[2], RquatTr[3], quatmagnitude(RquatTr));
+
+		// Flip y and z axies
+		FLT T2[4][4] = {
+			{ Tortho[0][0], -Tortho[0][2], -Tortho[0][1], 0.0 },
+			{ Tortho[1][0], -Tortho[1][2], -Tortho[1][1], 0.0 },
+			{ Tortho[2][0], -Tortho[2][2], -Tortho[2][1], 0.0 },
+			{          0.0,           0.0,           0.0, 1.0 } };
+PRINT(T2,4,4);
+
+		// Print out the quaternions
+		FLT T2quat[4];
+		quatfrommatrix(T2quat,   &T2[0][0] );
+		printf( "T2quat : %f %f %f %f = %f\n", T2quat [0], T2quat [1], T2quat [2], T2quat [3], quatmagnitude(T2quat));
+    }
+
+    // Run the calculation for Tcalc
+    //int run;
+    //for (run=0; run<100; run++) {
+/*
+    // Initialize Tcalc to the identity matrix
+    memcpy(Tcalc, Tortho, 4*4*sizeof(float));
+    //memset(Tcalc, 0, 4*4*sizeof(float));
+    //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
+
+    // Solve it!
+    AffineSolve(
+        Tcalc,           // OUTPUT: transform
+        O,  // INPUT:  points, offsets
+        N,  // INPUT:  plane normals
+        D,     // INPUT:  plane offsets
+        nPlanes, NITER,
+        STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
+        1);
+*/
+    // insert code here...
+    return 0;
+}
+
+
+
+
+
+#if 0
 #define PRINT_MAT(A,M,N) { \
     int m,n; \
     printf(#A "\n"); \
@@ -433,308 +780,4 @@ PRINT(ab,2,1);
 //    printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]);
 //    printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist);
 }
-
-void AffineSolve(
-    float T[4][4],           // OUTPUT: transform
-    float O[MAX_POINTS][4],  // INPUT:  points, offsets
-    float N[MAX_POINTS][3],  // INPUT:  plane normals
-    float D[MAX_POINTS],     // INPUT:  plane offsets
-    int nPoints, int nIter,
-    float stepSizeRot, float stepSizePos, float falloff, int constrain)
-{
-    int i,j,k,iter;
-    //T[3][3] = 1.0f;
-    
-    printf("iter x y z error\n");
-    
-    float gradDot     = 1.0;
-    float prevGradDot = 1.0;
-    float de_dT[3][4];  // the gradient
-    float conj[3][4];   // the conjugate
-    float errorSq=0.0;
-    for (iter=0; iter<nIter; iter++)
-    {
-        //----------------------------------
-        // Calculate the gradient direction
-        //----------------------------------
-        errorSq = 0.0;
-        memset(de_dT, 0, 3*4*sizeof(float));
-        for (i=0; i<nPoints; i++)
-        {
-            // What is the plane deviation error
-            float Ei = -D[i];
-            for (j=0; j<3; j++) {
-                float Tj_oi = 0.0f;
-                for (k=0; k<4; k++) {
-                    Tj_oi += T[j][k] * O[i][k];
-                }
-                Ei += N[i][j] * Tj_oi;
-            }
-//            printf("E[%d] %f\n", i, Ei);
-        
-            // Figure out contribution to the error
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    de_dT[j][k] += N[i][j] * O[i][k] * Ei;
-                }
-            }
-            
-            errorSq += Ei*Ei;
-        }
-
-//        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
-//exit(1);
-        // Constrain the gradient (such that dot products are zero)
-        if (constrain)
-        {
-            float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
-            for (k=0; k<3; k++) {
-                T0T1 += T[0][k] * T[1][k];
-                T1T2 += T[1][k] * T[2][k];
-                T2T0 += T[2][k] * T[0][k];
-            }
-//            printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
-            for (k=0; k<3; k++) {
-                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
-                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
-                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
-                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
-                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
-                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
-            }
-        }
-
-        // Calculate the gradient dot product
-        //  (used by conjugate gradient method)
-        prevGradDot = gradDot;
-        gradDot = 0.0;
-        for (j=0; j<3; j++) {
-            for (k=0; k<4; k++) {
-                gradDot += de_dT[j][k] * de_dT[j][k];
-            }
-        }
-
-//        printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
-
-        //----------------------------------
-        // Calculate the conjugate direction
-        //----------------------------------
-//        if (iter==0) {
-            // First iteration, just use the gradient
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    conj[j][k] = -de_dT[j][k];
-                }
-            }
-/*        } else {
-            // Calculate "beta" for Fletcher Reeves method
-            float beta = gradDot / prevGradDot;
-//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
-
-            // Update the conjugate
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    conj[j][k] = beta*conj[j][k] - de_dT[j][k];
-                }
-            }            
-        }
-*/
-
-//        PRINT_MAT(de_dT,4,4);
-//        exit(1);
-
-        //----------------------------------
-        // How large is the gradient ?
-        //----------------------------------
-        
-        double gradSizeRot = 0.0;
-        double gradSizePos = 0.0;
-        for (j=0; j<3; j++) {
-            for (k=0; k<3; k++) {
-                gradSizeRot += _ABS(conj[j][k]);
-            }
-            gradSizePos += _ABS(conj[j][k]);
-        }
-        if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; }  // Quit, we've totally converged
-        
-        //----------------------------------
-        // Descend in the gradient direction
-        //----------------------------------
-        if (gradSizeRot > TOO_SMALL) {
-            float scaleRot = stepSizeRot / gradSizeRot;
-            for (j=0; j<3; j++) {
-                for (k=0; k<3; k++) {
-                    T[j][k] += scaleRot * conj[j][k];
-                }
-            }
-            stepSizeRot *= falloff;
-        }
-        
-        if (gradSizePos > TOO_SMALL) {
-            float scalePos = stepSizePos / gradSizePos;
-            for (j=0; j<3; j++) {
-                T[j][3] += scalePos * conj[j][3];
-            }
-            stepSizePos *= falloff;
-        }
-        
-        // Constrain the gradient (such that scaling is one)
-        if (constrain)
-        {
-            // Measure the scales
-            float len[3] = {0.0, 0.0, 0.0};
-            for (j=0; j<3; j++) {
-                double lenSq = 0.0;
-                for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
-                len[j] = sqrt(lenSq);
-            }
-            
-            // How far off is the scale?
-            float xzLen = 0.5 * (len[0] + len[2]);
-            if (xzLen > TOO_SMALL) {
-                float inv_xzLen = 1.0 / xzLen;
-                for (j=0; j<3; j++) {
-                    T[3][j] *= inv_xzLen;
-                }
-            }
-            
-            // Rescale the thing
-            for (j=0; j<3; j++)
-            {
-                if (len[j] > TOO_SMALL) {
-                    float inv_len = 1.0 / len[j];
-                    for (k=0; k<3; k++) { T[j][k] *= inv_len; }
-                }
-            }
-        }
-    }
-    float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]);
-    printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist);
-}
-
-int main()
-{
-    int i,j,k,sen,axis;
-        
-    // Read the data files
-    ReadHmdPoints();
-    ReadPtinfo();
-
-    //-------------------------
-    // Package the lighthouse data for "AffineSolve"
-    //-------------------------
-
-    // Data for the "iterative" affine solve formula
-    float Tcalc[4][4];
-    float O[MAX_POINTS][4];
-    float N[MAX_POINTS][3];
-    float D[MAX_POINTS];
-    int nPlanes = 0;
-
-    for (sen=0; sen<NUM_HMD; sen++)
-    {
-        for (axis=0; axis<2; axis++)
-        {
-            if (hmd_angle[sen][axis] != -9999.0)
-            {
-                // Set the offset
-                O[nPlanes][0] = hmd_pos[sen][0];
-                O[nPlanes][1] = hmd_pos[sen][1];
-                O[nPlanes][2] = hmd_pos[sen][2];
-                O[nPlanes][3] = 1.0;
-        
-                // Calculate the plane equation
-                if (axis == 0) {   // Horizontal
-                    N[nPlanes][0] = -cos(hmd_angle[sen][axis]);
-                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
-                    N[nPlanes][2] = 0.0;
-                    D[nPlanes]    = 0.0;
-                } else {           // Vertical
-                    N[nPlanes][0] = 0.0;
-                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
-                    N[nPlanes][2] =  cos(hmd_angle[sen][axis]);
-                    D[nPlanes]    = 0.0;
-                }
-
-                printf("plane %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
-                    nPlanes,
-                    O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], 
-                    N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], 
-                    D[nPlanes]);
-                nPlanes++;
-            }
-        }
-    }
-
-    
-    printf("nPlanes %d\n", nPlanes);
-        
-    //}
-    
-    PRINT_MAT(Tcalc,4,4);
-
-    
-    //--------------------------------------------------
-    // Package the data for "OrthoSolve"
-    //--------------------------------------------------
-
-    // Data for the "fake" ortho solve formula
-    float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
-    FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
-    FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
-    FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
-    int nPoints=0;  
-
-    // Transform into the "OrthoSolve" format
-    for (sen=0; sen<NUM_HMD; sen++)
-    {
-        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
-        {
-            S_in[0][nPoints] = hmd_angle[sen][0];
-            S_in[1][nPoints] = hmd_angle[sen][1];
-            X_in[0][nPoints] = hmd_pos[sen][0];
-            X_in[1][nPoints] = hmd_pos[sen][1];
-            X_in[2][nPoints] = hmd_pos[sen][2];
-            nPoints++;
-        }
-    }
-    printf("OrthoSolve nPoints %d\n", nPoints);
-    
-    //--------------------------------------------------
-    // Run the "OrthoSolve" and then the "AffineSolve"
-    //--------------------------------------------------    
-    
-    int loop;
-    for (loop=0; loop<1; loop++)
-    {
-        // Run OrthoSolve
-        OrthoSolve(
-            Tortho,     // OUTPUT: 4x4 transformation matrix
-            S_out,      // OUTPUT: array of output screenspace points
-            S_in,       // INPUT:  array of screenspace points
-            X_in,       // INPUT:  array of offsets
-            nPoints);
-    }
-
-    // Run the calculation for Tcalc
-    //int run;
-    //for (run=0; run<100; run++) {
-/*
-    // Initialize Tcalc to the identity matrix
-    memcpy(Tcalc, Tortho, 4*4*sizeof(float));
-    //memset(Tcalc, 0, 4*4*sizeof(float));
-    //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
-
-    // Solve it!
-    AffineSolve(
-        Tcalc,           // OUTPUT: transform
-        O,  // INPUT:  points, offsets
-        N,  // INPUT:  plane normals
-        D,     // INPUT:  plane offsets
-        nPlanes, NITER,
-        STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
-        1);
-*/
-    // insert code here...
-    return 0;
-}
+#endif
diff --git a/dave/Makefile b/dave/Makefile
index bf62837..330bda3 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,7 +1,21 @@
+UNAME:=$(shell uname)
+
+CFLAGS:= -lm -I../redist -I../src -I../include/libsurvive
+
+ifeq ($(UNAME), Linux)
+CFLAGS:= $(CFLAGS) -lGL -lGLU -lglut -lX11 -I../../redist -DLINUX -lm -lpthread -DLINUX
+endif
+
+# Darwin is Mac OSX !!
+ifeq ($(UNAME), Darwin)
+CFLAGS:= $(CFLAGS) -I../../redist -w -framework OpenGL -framework GLUT
+endif
+
+
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
-	gcc -O3 -o dclapack_test dclapack_test.c -lm
-	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall
-
+	gcc -O3 -o dclapack_test dclapack_test.c $(CFLAGS)
+	gcc  -O3 -o AffineSolve   AffineSolve.c $(CFLAGS) ../redist/linmath.c #-Wall
+	gcc -O3 -o OrthoPlot OrthoPlot.c fileutil.c ../redist/linmath.c ../redist/os_generic.c $(CFLAGS)
 clean:
 	rm -f kalman_filter dclapack_test
diff --git a/dave/OrthoPlot.c b/dave/OrthoPlot.c
new file mode 100644
index 0000000..222828c
--- /dev/null
+++ b/dave/OrthoPlot.c
@@ -0,0 +1,451 @@
+/*
+ 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 "os_generic.h"
+#include "linmath.h"
+#include "fileutil.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
+#ifdef __linux__
+#include <GL/freeglut.h>
+#endif
+
+#define RegisterDriver(a,b)
+#include "poser_daveortho.c"
+
+
+// Required to set up a window
+#define WIDTH  1280
+#define HEIGHT 1280
+#define FULLSCREEN 0
+int    keys[256];               // Regular keyboard keys
+int sp_keys[256];               // Special keyboard keycodes (GLUT specific)
+
+#define LH_ID    0
+#define NUM_HMD 32
+#define INDIR "dave/full_test_triangle_on_floor/"
+#define MAX_POINTS SENSORS_PER_OBJECT
+
+#define PI 3.1415926535897932386264
+#define MOVESPEED 1.0
+#define ROTSPEED  5.0
+
+// 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;
+
+// Data for the "fake" ortho solve formula
+float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
+FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
+FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
+FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
+int nPoints=0;  
+
+//--------------------------------------------------------------------
+// 
+//--------------------------------------------------------------------
+
+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);
+
+
+float hmd_pos[NUM_HMD][3];
+void ReadHmdPoints()
+{
+    int i;
+    FILE *fin = fopen(INDIR "HMD_points.csv","r");
+    if (fin==NULL) {
+        printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n");
+        exit(1);
+    }
+    
+    for (i=0; i<NUM_HMD; i++) {
+        fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2]));
+    }
+    
+    fclose(fin);
+}
+
+float hmd_angle[NUM_HMD][2];
+void ReadPtinfo()
+{
+    // Initialize to -9999
+    int i;
+    for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; }
+
+    // Read ptinfo.csv
+    FILE *fin = fopen(INDIR "ptinfo.csv", "r");
+    if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
+    while (!feof(fin))
+    {
+        // Read the angle
+        int sen,lh,axis,count;
+        float angle, avglen, stddevang, stddevlen;
+        float max_outlier_length, max_outlier_angle;
+        int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n",
+                        &sen, &lh, &axis, &count,
+                        &angle, &avglen, &stddevang, &stddevlen,
+                        &max_outlier_length, &max_outlier_angle);
+        if (rt != 10) { break; }
+        
+        // If it's valid, store in the result
+        if (lh == LH_ID && sen < NUM_HMD) {
+            hmd_angle[sen][axis] = angle;
+        }
+    }
+    fclose(fin);
+}
+
+
+//--------------------------------------------------------------------
+// 
+//--------------------------------------------------------------------
+
+/*
+ * init() is called at program startup
+ */
+void init()
+{
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    ReadHmdPoints();
+    ReadPtinfo();
+
+    //--------------------------------------------------
+    // Package the data for "OrthoSolve"
+    //--------------------------------------------------
+
+    // Transform into the "OrthoSolve" format
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
+        {
+            S_in[0][nPoints] = hmd_angle[sen][0];
+            S_in[1][nPoints] = hmd_angle[sen][1];
+            X_in[0][nPoints] = hmd_pos[sen][0];
+            X_in[1][nPoints] = hmd_pos[sen][1];
+            X_in[2][nPoints] = hmd_pos[sen][2];
+            nPoints++;
+        }
+    }
+    printf("OrthoSolve nPoints %d\n", nPoints);
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    // Run OrthoSolve
+    OrthoSolve(
+		Tortho,     // OUTPUT: 4x4 transformation matrix
+		S_out,      // OUTPUT: array of output screenspace points
+		S_in,       // INPUT:  array of screenspace points
+		X_in,       // INPUT:  array of offsets
+		nPoints);
+		//printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+	//--------------------------------------------------
+	// Spawn a thread to read the HMD angles
+	//--------------------------------------------------
+	OGCreateThread(ThreadReadHmtAngles,0);
+
+    //--------------------------------------------------
+	// Initialize OpenGL
+    //--------------------------------------------------
+	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
+	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);				// 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    
+
+	// Bouning box around the points (in radians)
+	float x0=-45.0 * (PI/180.0);
+	float x1= 45.0 * (PI/180.0);
+	float y0=-45.0 * (PI/180.0);
+	float y1= 45.0 * (PI/180.0);
+
+
+	//------------------------
+	// Read the angles from stdin
+	//------------------------
+	FLT hmdAngles[4][NUM_HMD];
+
+	// Read the hmd angles
+	OGLockMutex(read_mutex);
+	for (i=0; i<4; i++) {
+		for (j=0; j<NUM_HMD; j++) {
+			hmdAngles[i][j] = read_hmdAngles[i][j];
+		}
+	}
+	OGUnlockMutex(read_mutex);
+
+	// Draw the hmd bearing angles
+	nPoints=0;
+	for (i=0; i<NUM_HMD; i++) {
+		const double dist=10.0;
+		
+		int sweepx = (LH_ID==0) ? SWEEP_LX : SWEEP_RX;
+		int sweepy = (LH_ID==0) ? SWEEP_LY : SWEEP_RY;
+
+		// If the left lighthouse sees it
+		if (read_hmdAngleViewed[sweepx][i] >= read_frameno-6 &&
+		    read_hmdAngleViewed[sweepy][i] >= read_frameno-6 &&
+		    hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0)
+		{
+			S_in[0][nPoints] = hmdAngles[sweepy][i];
+			S_in[1][nPoints] = hmdAngles[sweepx][i];
+            X_in[0][nPoints] = hmd_pos[i][0];
+            X_in[1][nPoints] = hmd_pos[i][1];
+            X_in[2][nPoints] = hmd_pos[i][2];
+printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n",
+	i, S_in[0][nPoints], S_in[1][nPoints],
+	X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints],
+	read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno);
+			nPoints++;
+		}
+	}
+
+	read_frameno++;
+
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    // Run OrthoSolve
+    OrthoSolve(
+		Tortho,     // OUTPUT: 4x4 transformation matrix
+		S_out,      // OUTPUT: array of output screenspace points
+		S_in,       // INPUT:  array of screenspace points
+		X_in,       // INPUT:  array of offsets
+		nPoints);
+		printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+
+	//------------------------
+	// Draw the inputs
+	//------------------------
+	glPointSize(3.0);
+
+	// Draw the input points
+	glColor3f(1.0,0.5,0.5);
+	glBegin(GL_POINTS);
+	for (i=0; i<nPoints; i++) {
+		glVertex2f(  (S_in[0][i]-x0)/(x1-x0),  (S_in[1][i]-y0)/(y1-y0)  );
+	}
+	glEnd();
+
+	// Draw the output points
+	glColor3f(0.5,0.5,1.0);
+	glBegin(GL_POINTS);
+	for (i=0; i<nPoints; i++) {
+		glVertex2f(  (S_out[0][i]-x0)/(x1-x0),  (S_out[1][i]-y0)/(y1-y0)  );
+	}
+	glEnd();
+
+
+    //
+    // We're Done
+    //
+    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, 1.0, 0.0, 1.0, -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;
+}
+
+
+
+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)
+{
+	FLT i0[3],j0[3],k0[3];
+	FLT i[3],j[3],k[3];
+	FLT q[4];
+	
+	i0[0]=1.0; i0[1]=0.0; i0[2]=0.0;
+	j0[0]=0.0; j0[1]=1.0; j0[2]=0.0;
+	k0[0]=0.0; k0[1]=0.0; k0[2]=1.0;
+	q [0]=qr;  q [1]=qx;  q [2]=qy;  q [3]=qz;
+	
+	quatrotatevector(i, q, i0);
+	quatrotatevector(j, q, j0);
+	quatrotatevector(k, q, k0);
+
+	glBegin(GL_LINES);
+	glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i[0],z+i[2],y+i[1]);
+	glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j[0],z+j[2],y+j[1]);
+	glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k[0],z+k[2],y+k[1]);
+	glEnd();
+}
+
+
diff --git a/dave/dclapack_test.c b/dave/dclapack_test.c
index b0667ef..3f48b08 100644
--- a/dave/dclapack_test.c
+++ b/dave/dclapack_test.c
@@ -1,6 +1,6 @@
 #define FLOAT float
 #define ORDER 50
-#include "dclapack.h"
+#include "../redist/dclapack.h"
 
 #include <stdio.h>
 #include <stdlib.h>
diff --git a/dave/fileutil.c b/dave/fileutil.c
new file mode 100644
index 0000000..04dc241
--- /dev/null
+++ b/dave/fileutil.c
@@ -0,0 +1,133 @@
+#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) {
+		int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+		if (rt==8)
+		{
+			//int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+			//printf( "%s %s %s %f %d %d %d %d\n", house, xy, hmd, ts,id, syncid, timeInSweep, length );
+
+			if( id < 0 ) continue;
+			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 ) { id += 0;  } 
+			else if ( strcmp(hmd,"WM0")==0 ) { id += 32; }
+			else if ( strcmp(hmd,"WM1")==0 ) { id += 56; }
+			else    { 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/fileutil.h b/dave/fileutil.h
new file mode 100644
index 0000000..e5da244
--- /dev/null
+++ b/dave/fileutil.h
@@ -0,0 +1,35 @@
+#ifndef _fileutil_h_
+#define _fileutil_h_
+
+#include <pthread.h>
+#include "os_generic.h"
+
+void LoadLighthousePos(
+	const char *path, 
+	float *x, float *y, float *z,
+	float *qi, float *qj, float *qk, float *qreal);
+
+
+// first 32 are hmd, next 24 wm0 next 24 wm1
+#define NUM_HMD   80
+#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_
+
+