diff options
Diffstat (limited to 'dave/OrthoPlot.c')
-rw-r--r-- | dave/OrthoPlot.c | 451 |
1 files changed, 0 insertions, 451 deletions
diff --git a/dave/OrthoPlot.c b/dave/OrthoPlot.c deleted file mode 100644 index 222828c..0000000 --- a/dave/OrthoPlot.c +++ /dev/null @@ -1,451 +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 "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(); -} - - |