mirror of
https://github.com/collabora/libsurvive.git
synced 2026-08-04 00:51:21 -04:00
127 lines
2.8 KiB
C
127 lines
2.8 KiB
C
#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 ) { id += 0; }
|
|
else if ( strcmp(hmd,"WM0")==0 ) { id += 32; }
|
|
else if ( strcmp(hmd,"WM1")==0 ) { id += 56; }
|
|
else { 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;
|
|
}
|
|
}
|
|
|
|
|