Merge pull request #8 from cnlohr/master

Updating Master

7 files changed, 74 insertions, 51 deletions

diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile
index d156bdb..db382ea 100644
--- a/tools/plot_lighthouse/Makefile
+++ b/tools/plot_lighthouse/Makefile
@@ -1,7 +1,7 @@
 UNAME := $(shell uname)
 
 ifeq ($(UNAME), Linux)
-CFLAGS:= -lGL -lGLU -lglut
+CFLAGS:= -lGL -lGLU -lglut -I../../redist -DLINUX -lm -lpthread
 endif
 
 # Darwin is Mac OSX !!
@@ -10,6 +10,6 @@ CFLAGS:= -w -framework OpenGL -framework GLUT
 endif
 
 all:
-	gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c $(CFLAGS)
+	gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c ../../redist/linmath.c $(CFLAGS)
 clean:
 	rm -f plot_lighthouse
diff --git a/tools/plot_lighthouse/fileutil.c b/tools/plot_lighthouse/fileutil.c
index f892798..b2c8f64 100644
--- a/tools/plot_lighthouse/fileutil.c
+++ b/tools/plot_lighthouse/fileutil.c
@@ -110,7 +110,10 @@ void *ThreadReadHmtAngles(void *junk)
 		if ( xy[0]   =='Y' ) { sweepId++; }
 		double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 );
 		
-		if ( strcmp(hmd,"HMD")!=0 ) { continue; }
+		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; }
 
diff --git a/tools/plot_lighthouse/fileutil.h b/tools/plot_lighthouse/fileutil.h
index 714fbbd..e5da244 100644
--- a/tools/plot_lighthouse/fileutil.h
+++ b/tools/plot_lighthouse/fileutil.h
@@ -2,7 +2,7 @@
 #define _fileutil_h_
 
 #include <pthread.h>
-#include "../../redist/os_generic.h"
+#include "os_generic.h"
 
 void LoadLighthousePos(
 	const char *path, 
@@ -10,7 +10,8 @@ void LoadLighthousePos(
 	float *qi, float *qj, float *qk, float *qreal);
 
 
-#define NUM_HMD   32
+// 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
diff --git a/tools/plot_lighthouse/glutil.c b/tools/plot_lighthouse/glutil.c
index dd022a0..99c15f7 100644
--- a/tools/plot_lighthouse/glutil.c
+++ b/tools/plot_lighthouse/glutil.c
@@ -50,24 +50,23 @@ 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);
+	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;
 	
-	// Draw the coordinate system i red, j green, k blue
+	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.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);
+	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/tools/plot_lighthouse/glutil.h b/tools/plot_lighthouse/glutil.h
index 1014506..936701e 100644
--- a/tools/plot_lighthouse/glutil.h
+++ b/tools/plot_lighthouse/glutil.h
@@ -10,7 +10,8 @@
 #define glutil_h
 
 #include <stdio.h>
-#include "quaternion.h"
+//#include "quaternion.h"
+#include "linmath.h"
 
 #include <stdio.h>
 #ifdef __APPLE__
diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c
index c2fd97a..4a64c28 100644
--- a/tools/plot_lighthouse/main.c
+++ b/tools/plot_lighthouse/main.c
@@ -13,6 +13,11 @@
 #include "glutil.h"
 #include "fileutil.h"
 
+#ifdef LINUX
+#include <GL/freeglut.h>
+#endif
+
+
 // Required to set up a window
 #define WIDTH  800
 #define HEIGHT 600
@@ -73,7 +78,8 @@ float rotz=0.0f;
 void init()
 {    
 	int i,j;
-    
+//void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
+
 	// 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);
@@ -86,14 +92,14 @@ void init()
 	// 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 (i=0; i<NUM_HMD; i++) {
 		for (j=0; j<4; j++) {
 			read_hmdAngles[j][i] = -9999.0;
 		}
 	}
 	read_thread = OGCreateThread(ThreadReadHmtAngles,NULL);	
 
-	for (i=0; i<32; i++) {
+	for (i=0; i<NUM_HMD; 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",
@@ -220,43 +226,55 @@ void draw()
 	// Read the hmd angles
 	OGLockMutex(read_mutex);
 	for (i=0; i<4; i++) {
-		for (j=0; j<32; j++) {
+		for (j=0; j<NUM_HMD; j++) {
 			hmdAngles[i][j] = read_hmdAngles[i][j];
 		}
 	}
 	OGUnlockMutex(read_mutex);
 
-	// For every head mount angle
+	// Draw the hmd bearing angles
 	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)
+	for (i=0; i<NUM_HMD; i++) {
+		const double dist=10.0;
+		
+		// If the left lighthouse sees it
+		if (read_hmdAngleViewed[SWEEP_LX][i] >= read_frameno-6 &&
+		    read_hmdAngleViewed[SWEEP_LY][i] >= read_frameno-6 &&
+		    hmdAngles[SWEEP_LX][i]!=-9999.0 && hmdAngles[SWEEP_LY][i]!=-9999.0)
 		{
 			// Get the hmd bearings
-			double Ldx,Ldy,Ldz,Rdx,Rdy,Rdz;
+			double Ldx,Ldy,Ldz;
 			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
-		
+
+			// Rotate the bearings by the lighthouse orientation
+			FLT L0[3],L[3],Lq[4];
+			L0[0]=Ldx; L0[1]=Ldy; L0[2]=Ldz;
+			Lq[0]=houseL.qreal; Lq[1]=houseL.qi; Lq[2]=houseL.qj; Lq[3]=houseL.qk;
+			quatrotatevector(L, Lq, L0);
+
 			// 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(houseL.x+dist*L[0], houseL.z+dist*L[2], houseL.y+dist*L[1]);
+		}
+		
+		// If the left lighthouse sees it
+		if (read_hmdAngleViewed[SWEEP_RX][i] >= read_frameno-6 &&
+		    read_hmdAngleViewed[SWEEP_RY][i] >= read_frameno-6 &&
+		    hmdAngles[SWEEP_RX][i]!=-9999.0 && hmdAngles[SWEEP_RY][i]!=-9999.0)
+		{
+			// Get the hmd bearings
+			double Rdx,Rdy,Rdz;
+			HmdDir(hmdAngles[SWEEP_RX][i], hmdAngles[SWEEP_RY][i], &Rdx, &Rdy, &Rdz);
+
+			// Rotate the bearings by the lighthouse orientation
+			FLT R0[3],R[3],Rq[4];
+			R0[0]=Rdx; R0[1]=Rdy; R0[2]=Rdz;
+			Rq[0]=houseR.qreal; Rq[1]=houseR.qi; Rq[2]=houseR.qj; Rq[3]=houseR.qk;
+			quatrotatevector(R, Rq, R0);
+
+			// Plot the lines
 			glVertex3f(houseR.x, houseR.z, houseR.y);
-			glVertex3f(houseR.x+dist*R.i, houseR.z+dist*R.k, houseR.y+dist*R.j);
+			glVertex3f(houseR.x+dist*R[0], houseR.z+dist*R[2], houseR.y+dist*R[1]);
 		}
 	}
 	glEnd();
diff --git a/tools/process_rawcap/process_to_points.c b/tools/process_rawcap/process_to_points.c
index a1e835d..3ccfcc1 100644
--- a/tools/process_rawcap/process_to_points.c
+++ b/tools/process_rawcap/process_to_points.c
@@ -192,7 +192,8 @@ int main( int argc, char ** argv )
 			stddevtim += Sdiff * Sdiff;
 			stddevlen += Ldiff * Ldiff;
 
-			Sdiff/=4;
+			//Cast a wider net for the histogram.
+			//Sdiff/=4;
 
 			int llm = Sdiff + (HISTOGRAMSIZE/2.0);
 			if( llm < 0 ) llm = 0;