diff options
Diffstat (limited to 'dave/plot_lighthouse/main.c')
-rwxr-xr-x | dave/plot_lighthouse/main.c | 336 |
1 files changed, 0 insertions, 336 deletions
diff --git a/dave/plot_lighthouse/main.c b/dave/plot_lighthouse/main.c deleted file mode 100755 index c2fd97a..0000000 --- a/dave/plot_lighthouse/main.c +++ /dev/null @@ -1,336 +0,0 @@ -/* - 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; -} - |