diff options
Diffstat (limited to 'dave/plot_lighthouse/fileutil.c')
-rw-r--r-- | dave/plot_lighthouse/fileutil.c | 124 |
1 files changed, 124 insertions, 0 deletions
diff --git a/dave/plot_lighthouse/fileutil.c b/dave/plot_lighthouse/fileutil.c new file mode 100644 index 0000000..f892798 --- /dev/null +++ b/dave/plot_lighthouse/fileutil.c @@ -0,0 +1,124 @@ +#include "fileutil.h" +#include <stdio.h> +#include <stdlib.h> + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; i<NUM_SWEEP; i++) { + for (j=0; j<NUM_HMD; j++) { + angles[i][j] = -9999.0; // Initially no value + } + } + + FILE *f = fopen_orDie(path, "r"); + + while (!feof(f)) + { + // Read the line from the file + int rt=fscanf(f, "%s %s %d %d %lf %lf %lf %lf", + &type, &sweep, &id, &nSweep, &ang, + &d1,&d2,&d3); + + if (rt<8) { break; } + + // Only hmd points + if ( strcmp(type,"HMD")!=0 ) { continue; } + + // Which sweep is it? + int sweepId=-1; + if ( strcmp(sweep,"LX")==0 ) { sweepId=SWEEP_LX; } + else if ( strcmp(sweep,"LY")==0 ) { sweepId=SWEEP_LY; } + else if ( strcmp(sweep,"RX")==0 ) { sweepId=SWEEP_RX; } + else if ( strcmp(sweep,"RY")==0 ) { sweepId=SWEEP_RY; } + else { continue; } + + // Convert the angle from ticks to radians + angles[sweepId][id] = (PI / 400000.0) * ( ang-200000.0 ); + } + + fclose(f); +} + +void *ThreadReadHmtAngles(void *junk) +{ + char house[256]; + char xy[256]; + char hmd[256]; + double ts; + int id; + int syncid; + int timeInSweep; + int length; + //lighthouse sweep hmdOrwmd timestamp id syncid timeinsweep length + + while(1) { + scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length); + + int sweepId=0; + if ( house[0]=='R' ) { sweepId+=2; } + if ( xy[0] =='Y' ) { sweepId++; } + double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 ); + + if ( strcmp(hmd,"HMD")!=0 ) { continue; } + + if ( id<0 || id >NUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } +} + + |