Replaced rotation algorithm & cleanup

1 files changed, 166 insertions, 168 deletions

diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c
index 14d9423..aa51448 100644
--- a/tools/lighthousefind_tori/main.c
+++ b/tools/lighthousefind_tori/main.c
@@ -21,52 +21,52 @@ int best_hmd_target = 0;
 
 static void printTrackedObject(TrackedObject *to)
 {
-    for (unsigned int i = 0; i < to->numSensors; i++)
-    {
-        printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n",
-            i,
-            to->sensor[i].theta,
-            to->sensor[i].phi,
-            to->sensor[i].point.x,
-            to->sensor[i].point.y,
-            to->sensor[i].point.z
-            );
-    }
+	for (unsigned int i = 0; i < to->numSensors; i++)
+	{
+		printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n",
+			i,
+			to->sensor[i].theta,
+			to->sensor[i].phi,
+			to->sensor[i].point.x,
+			to->sensor[i].point.y,
+			to->sensor[i].point.z
+			);
+	}
 }
 
 static void runTheNumbers()
 {
-    TrackedObject *to;
+	TrackedObject *to;
 
-    to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor)));
+	to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor)));
 
-    int sensorCount = 0;
+	int sensorCount = 0;
 
-    for (int i = 0; i < PTS; i++)
-    {
-        // if there are enough valid counts for both the x and y sweeps for sensor i
-        if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) &&
-            (hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID))
-        {
-            to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0];
-            to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1];
-            to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2];
-            to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0];
-            to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1];
-            sensorCount++;
-        }
-    }
+	for (int i = 0; i < PTS; i++)
+	{
+		// if there are enough valid counts for both the x and y sweeps for sensor i
+		if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) &&
+			(hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID))
+		{
+			to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0];
+			to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1];
+			to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2];
+			to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0] + LINMATHPI / 2;
+			to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2;
+			sensorCount++;
+		}
+	}
 
-    to->numSensors = sensorCount;
+	to->numSensors = sensorCount;
 
-    printf("Using %d sensors to find lighthouse.\n", sensorCount);
+	printf("Using %d sensors to find lighthouse.\n", sensorCount);
 
-    Point lh = SolveForLighthouse(to, 1);
+	Point lh = SolveForLighthouse(to, 1);
 
-    printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z);
+	printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z);
 
-    //printTrackedObject(to);
-    free(to);
+	//printTrackedObject(to);
+	free(to);
 }
 
 
@@ -74,146 +74,144 @@ int LoadData(char Camera, const char * datafile)
 {
 
 
-    //First, read the positions of all the sensors on the HMD.
-    FILE * f = fopen("HMD_points.csv", "r");
-    int pt = 0;
-    if (!f) 
-    { 
-        fprintf(stderr, "error: can't open hmd points.\n"); 
-        return -5; 
-    }
-    while (!feof(f) && !ferror(f) && pt < PTS)
-    {
-        float fx, fy, fz;
-        int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz);
-        hmd_points[pt * 3 + 0] = fx;
-        hmd_points[pt * 3 + 1] = fy;
-        hmd_points[pt * 3 + 2] = fz;
-        pt++;
-        if (r != 3)
-        {
-            fprintf(stderr, "Not enough entries on line %d of points\n", pt);
-            return -8;
-        }
-    }
-    if (pt < PTS)
-    {
-        fprintf(stderr, "Not enough points.\n");
-        return -9;
-    }
-    fclose(f);
-    printf("Loaded %d points\n", pt);
-
-    //Read all the normals on the HMD into hmd_norms.
-    f = fopen("HMD_normals.csv", "r");
-    int nrm = 0;
-    if (!f) 
-    { 
-        fprintf(stderr, "error: can't open hmd points.\n"); 
-        return -5; 
-    }
-    while (!feof(f) && !ferror(f) && nrm < PTS)
-    {
-        float fa, fb, fc;
-        int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc);
-        hmd_norms[nrm * 3 + 0] = fa;
-        hmd_norms[nrm * 3 + 1] = fb;
-        hmd_norms[nrm * 3 + 2] = fc;
-        nrm++;
-        if (r != 3)
-        {
-            fprintf(stderr, "Not enough entries on line %d of normals\n", nrm);
-            return -8;
-        }
-    }
-    if (nrm < PTS)
-    {
-        fprintf(stderr, "Not enough points.\n");
-        return -9;
-    }
-    if (nrm != pt)
-    {
-        fprintf(stderr, "point/normal counts disagree.\n");
-        return -9;
-    }
-    fclose(f);
-    printf("Loaded %d norms\n", nrm);
-
-    //Actually load the processed data!
-    int xck = 0;
-    f = fopen(datafile, "r");
-    if (!f)
-    {
-        fprintf(stderr, "Error: cannot open %s\n", datafile);
-        exit(-11);
-    }
-    int lineno = 0;
-    while (!feof(f))
-    {
-        //Format:
-        // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936
-        lineno++;
-        char devn[10];
-        char inn[10];
-        int id;
-        int pointct;
-        double avgTime;
-        double avgLen;
-        double stddevTime;
-        double stddevLen;
-        int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen);
-        if (ct == 0) continue;
-        if (ct != 8)
-        {
-            fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno);
-        }
-        if (strcmp(devn, "HMD") != 0) continue;
-
-        if (inn[0] != Camera) continue;
-
-        int isy = inn[1] == 'Y';
-
-//        hmd_point_angles[id * 2 + isy] = (avgTime - 200000) / 200000 * 3.1415926535 / 2.0;
-        hmd_point_angles[id * 2 + isy] = (FLT)(avgTime / 48000000 * 60 * M_PI * 2);
-        
-        hmd_point_counts[id * 2 + isy] = pointct;
-    }
-    fclose(f);
-
-
-    int targpd;
-    int maxhits = 0;
-
-    for (targpd = 0; targpd < PTS; targpd++)
-    {
-        int hits = hmd_point_counts[targpd * 2 + 0];
-        if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1];
-        //Need an X and a Y lock.  
-
-        if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; }
-    }
-    if (maxhits < MIN_HITS_FOR_VALID)
-    {
-        fprintf(stderr, "Error: Not enough data for a primary fix.\n");
-    }
-
-    return 0;
+	//First, read the positions of all the sensors on the HMD.
+	FILE * f = fopen("HMD_points.csv", "r");
+	int pt = 0;
+	if (!f) 
+	{ 
+		fprintf(stderr, "error: can't open hmd points.\n"); 
+		return -5; 
+	}
+	while (!feof(f) && !ferror(f) && pt < PTS)
+	{
+		float fx, fy, fz;
+		int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz);
+		hmd_points[pt * 3 + 0] = fx;
+		hmd_points[pt * 3 + 1] = fy;
+		hmd_points[pt * 3 + 2] = fz;
+		pt++;
+		if (r != 3)
+		{
+			fprintf(stderr, "Not enough entries on line %d of points\n", pt);
+			return -8;
+		}
+	}
+	if (pt < PTS)
+	{
+		fprintf(stderr, "Not enough points.\n");
+		return -9;
+	}
+	fclose(f);
+	printf("Loaded %d points\n", pt);
+
+	//Read all the normals on the HMD into hmd_norms.
+	f = fopen("HMD_normals.csv", "r");
+	int nrm = 0;
+	if (!f) 
+	{ 
+		fprintf(stderr, "error: can't open hmd points.\n"); 
+		return -5; 
+	}
+	while (!feof(f) && !ferror(f) && nrm < PTS)
+	{
+		float fa, fb, fc;
+		int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc);
+		hmd_norms[nrm * 3 + 0] = fa;
+		hmd_norms[nrm * 3 + 1] = fb;
+		hmd_norms[nrm * 3 + 2] = fc;
+		nrm++;
+		if (r != 3)
+		{
+			fprintf(stderr, "Not enough entries on line %d of normals\n", nrm);
+			return -8;
+		}
+	}
+	if (nrm < PTS)
+	{
+		fprintf(stderr, "Not enough points.\n");
+		return -9;
+	}
+	if (nrm != pt)
+	{
+		fprintf(stderr, "point/normal counts disagree.\n");
+		return -9;
+	}
+	fclose(f);
+	printf("Loaded %d norms\n", nrm);
+
+	//Actually load the processed data!
+	int xck = 0;
+	f = fopen(datafile, "r");
+	if (!f)
+	{
+		fprintf(stderr, "Error: cannot open %s\n", datafile);
+		exit(-11);
+	}
+	int lineno = 0;
+	while (!feof(f))
+	{
+		//Format:
+		// HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936
+		lineno++;
+		char devn[10];
+		char inn[10];
+		int id;
+		int pointct;
+		double avgTime;
+		double avgLen;
+		double stddevTime;
+		double stddevLen;
+		int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen);
+		if (ct == 0) continue;
+		if (ct != 8)
+		{
+			fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno);
+		}
+		if (strcmp(devn, "HMD") != 0) continue;
+
+		if (inn[0] != Camera) continue;
+
+		int isy = inn[1] == 'Y';
+
+		hmd_point_angles[id * 2 + isy] = ((FLT)avgTime - 200000) * LINMATHPI / 200000 / 2;
+		
+		hmd_point_counts[id * 2 + isy] = pointct;
+	}
+	fclose(f);
+
+
+	int targpd;
+	int maxhits = 0;
+
+	for (targpd = 0; targpd < PTS; targpd++)
+	{
+		int hits = hmd_point_counts[targpd * 2 + 0];
+		if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1];
+		//Need an X and a Y lock.  
+
+		if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; }
+	}
+	if (maxhits < MIN_HITS_FOR_VALID)
+	{
+		fprintf(stderr, "Error: Not enough data for a primary fix.\n");
+	}
+
+	return 0;
 }
 
 
 int main(int argc, char ** argv)
 {
-    if (argc != 3)
-    {
-        fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n");
-        exit(-1);
-    }
+	if (argc != 3)
+	{
+		fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n");
+		exit(-1);
+	}
 
-    //Load either 'L' (LH1) or 'R' (LH2) data.
-    if (LoadData(argv[1][0], argv[2])) return 5;
+	//Load either 'L' (LH1) or 'R' (LH2) data.
+	if (LoadData(argv[1][0], argv[2])) return 5;
 
+	runTheNumbers();
 
-    runTheNumbers();
-
-    return 0;
+	return 0;
 }