From 0ab91a6e9374f190c049a5d8ea1319b9b37529c1 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sun, 15 Apr 2018 17:43:12 -0400
Subject: Move things into attic and update Makefile to do dependnencies.

---
 attic/dave/OrthoPlot.c | 451 +++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 451 insertions(+)
 create mode 100644 attic/dave/OrthoPlot.c

(limited to 'attic/dave/OrthoPlot.c')

diff --git a/attic/dave/OrthoPlot.c b/attic/dave/OrthoPlot.c
new file mode 100644
index 0000000..222828c
--- /dev/null
+++ b/attic/dave/OrthoPlot.c
@@ -0,0 +1,451 @@
+/*
+ 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();
+}
+
+
-- 
cgit v1.2.3