aboutsummaryrefslogtreecommitdiff
path: root/dave/OrthoPlot.c
diff options
context:
space:
mode:
Diffstat (limited to 'dave/OrthoPlot.c')
-rw-r--r--dave/OrthoPlot.c451
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();
-}
-
-