aboutsummaryrefslogtreecommitdiff
path: root/dave/OrthoPlot.c
blob: 222828c135aef876c140f5082dd96fc775fbe5b7 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
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();
}