aboutsummaryrefslogtreecommitdiff
path: root/dave/plot_lighthouse/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'dave/plot_lighthouse/main.c')
-rwxr-xr-xdave/plot_lighthouse/main.c336
1 files changed, 336 insertions, 0 deletions
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;
+}
+