Adding lighthousefind_tori

9 files changed, 1240 insertions, 0 deletions

diff --git a/tools/lighthousefind_tori/Makefile b/tools/lighthousefind_tori/Makefile
new file mode 100644
index 0000000..3bdfdd6
--- /dev/null
+++ b/tools/lighthousefind_tori/Makefile
@@ -0,0 +1,7 @@
+CFLAGS:=-g -O4 -DFLT=double -I../../redist -flto
+LDFLAGS:=$(CFLAGS) -lm
+
+all:
+	gcc -O3 -o lighthousefind-tori main.c find_tori_math.c torus_localizer.c visualization.c ../../redist/linmath.c $(LDFLAGS)
+clean:
+	rm -f lighthousefind-tori
diff --git a/tools/lighthousefind_tori/find_tori_math.c b/tools/lighthousefind_tori/find_tori_math.c
new file mode 100644
index 0000000..9c305f3
--- /dev/null
+++ b/tools/lighthousefind_tori/find_tori_math.c
@@ -0,0 +1,206 @@
+#include <math.h>
+#include <float.h>
+#include "find_tori_math.h"
+
+// TODO: optimization potential to do in-place inverse for some places where this is used.
+Matrix3x3 inverseM33(const Matrix3x3 mat)
+{
+    Matrix3x3 newMat;
+    for (int a = 0; a < 3; a++)
+    {
+        for (int b = 0; b < 3; b++)
+        {
+            newMat.val[a][b] = mat.val[a][b];
+        }
+    }
+
+    for (int i = 0; i < 3; i++)
+    {
+        for (int j = i + 1; j < 3; j++)
+        {
+            double tmp = newMat.val[i][j];
+            newMat.val[i][j] = newMat.val[j][i];
+            newMat.val[j][i] = tmp;
+        }
+    }
+
+    return newMat;
+}
+
+
+double distance(Point a, Point b)
+{
+    double x = a.x - b.x;
+    double y = a.y - b.y;
+    double z = a.z - b.z;
+    return sqrt(x*x + y*y + z*z);
+}
+
+//###################################
+// The following code originally came from 
+//  http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw
+// Need to check up on license terms and give proper attribution
+// I think we'll be good with proper attribution, but don't want to assume without checking.
+
+
+
+/* -------------------------------------------------------------------- */
+/* Math Lib declarations */
+
+
+
+/* -------------------------------------------------------------------- */
+/* Main function */
+
+/**
+* Calculate a rotation matrix from 2 normalized vectors.
+*
+* v1 and v2 must be unit length.
+*/
+void rotation_between_vecs_to_mat3(double m[3][3], const double v1[3], const double v2[3])
+{
+    double axis[3];
+    /* avoid calculating the angle */
+    double angle_sin;
+    double angle_cos;
+
+    cross_v3_v3v3(axis, v1, v2);
+
+    angle_sin = normalize_v3(axis);
+    angle_cos = dot_v3v3(v1, v2);
+
+    if (angle_sin > FLT_EPSILON) {
+    axis_calc:
+        axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
+    }
+    else {
+        /* Degenerate (co-linear) vectors */
+        if (angle_cos > 0.0f) {
+            /* Same vectors, zero rotation... */
+            unit_m3(m);
+        }
+        else {
+            /* Colinear but opposed vectors, 180 rotation... */
+            ortho_v3_v3(axis, v1);
+            normalize_v3(axis);
+            angle_sin = 0.0f;  /* sin(M_PI) */
+            angle_cos = -1.0f;  /* cos(M_PI) */
+            goto axis_calc;
+        }
+    }
+}
+
+
+/* -------------------------------------------------------------------- */
+/* Math Lib */
+
+void unit_m3(double m[3][3])
+{
+    m[0][0] = m[1][1] = m[2][2] = 1.0;
+    m[0][1] = m[0][2] = 0.0;
+    m[1][0] = m[1][2] = 0.0;
+    m[2][0] = m[2][1] = 0.0;
+}
+
+double dot_v3v3(const double a[3], const double b[3])
+{
+    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+}
+
+void cross_v3_v3v3(double r[3], const double a[3], const double b[3])
+{
+    r[0] = a[1] * b[2] - a[2] * b[1];
+    r[1] = a[2] * b[0] - a[0] * b[2];
+    r[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+void mul_v3_v3fl(double r[3], const double a[3], double f)
+{
+    r[0] = a[0] * f;
+    r[1] = a[1] * f;
+    r[2] = a[2] * f;
+}
+
+double normalize_v3_v3(double r[3], const double a[3])
+{
+    double d = dot_v3v3(a, a);
+
+    if (d > 1.0e-35f) {
+        d = sqrtf((float)d);
+        mul_v3_v3fl(r, a, 1.0f / d);
+    }
+    else {
+        d = r[0] = r[1] = r[2] = 0.0f;
+    }
+
+    return d;
+}
+
+double normalize_v3(double n[3])
+{
+    return normalize_v3_v3(n, n);
+}
+
+int axis_dominant_v3_single(const double vec[3])
+{
+    const float x = fabsf((float)vec[0]);
+    const float y = fabsf((float)vec[1]);
+    const float z = fabsf((float)vec[2]);
+    return ((x > y) ?
+        ((x > z) ? 0 : 2) :
+        ((y > z) ? 1 : 2));
+}
+
+void ortho_v3_v3(double p[3], const double v[3])
+{
+    const int axis = axis_dominant_v3_single(v);
+
+    switch (axis) {
+    case 0:
+        p[0] = -v[1] - v[2];
+        p[1] = v[0];
+        p[2] = v[0];
+        break;
+    case 1:
+        p[0] = v[1];
+        p[1] = -v[0] - v[2];
+        p[2] = v[1];
+        break;
+    case 2:
+        p[0] = v[2];
+        p[1] = v[2];
+        p[2] = -v[0] - v[1];
+        break;
+    }
+}
+
+/* axis must be unit length */
+void axis_angle_normalized_to_mat3_ex(
+    double mat[3][3], const double axis[3],
+    const double angle_sin, const double angle_cos)
+{
+    double nsi[3], ico;
+    double n_00, n_01, n_11, n_02, n_12, n_22;
+
+    ico = (1.0f - angle_cos);
+    nsi[0] = axis[0] * angle_sin;
+    nsi[1] = axis[1] * angle_sin;
+    nsi[2] = axis[2] * angle_sin;
+
+    n_00 = (axis[0] * axis[0]) * ico;
+    n_01 = (axis[0] * axis[1]) * ico;
+    n_11 = (axis[1] * axis[1]) * ico;
+    n_02 = (axis[0] * axis[2]) * ico;
+    n_12 = (axis[1] * axis[2]) * ico;
+    n_22 = (axis[2] * axis[2]) * ico;
+
+    mat[0][0] = n_00 + angle_cos;
+    mat[0][1] = n_01 + nsi[2];
+    mat[0][2] = n_02 - nsi[1];
+    mat[1][0] = n_01 - nsi[2];
+    mat[1][1] = n_11 + angle_cos;
+    mat[1][2] = n_12 + nsi[0];
+    mat[2][0] = n_02 + nsi[1];
+    mat[2][1] = n_12 - nsi[0];
+    mat[2][2] = n_22 + angle_cos;
+}
diff --git a/tools/lighthousefind_tori/find_tori_math.h b/tools/lighthousefind_tori/find_tori_math.h
new file mode 100644
index 0000000..a10c3fc
--- /dev/null
+++ b/tools/lighthousefind_tori/find_tori_math.h
@@ -0,0 +1,23 @@
+#ifndef __FIND_TORI_MATH_H
+#define __FIND_TORI_MATH_H
+
+#include "tori_includes.h"
+
+Matrix3x3 inverseM33(const Matrix3x3 mat);
+double distance(Point a, Point b);
+
+void unit_m3(double m[3][3]);
+double dot_v3v3(const double a[3], const double b[3]);
+double normalize_v3(double n[3]);
+void cross_v3_v3v3(double r[3], const double a[3], const double b[3]);
+void mul_v3_v3fl(double r[3], const double a[3], double f);
+void ortho_v3_v3(double p[3], const double v[3]);
+void axis_angle_normalized_to_mat3_ex(
+        double mat[3][3], 
+        const double axis[3],
+        const double angle_sin, 
+        const double angle_cos);
+void rotation_between_vecs_to_mat3(double m[3][3], const double v1[3], const double v2[3]);
+
+
+#endif
diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c
new file mode 100644
index 0000000..2fd517a
--- /dev/null
+++ b/tools/lighthousefind_tori/main.c
@@ -0,0 +1,219 @@
+#include <stdio.h>
+#include <assert.h>
+#include <memory.h>
+#include <time.h>
+#include <string.h>
+#include <stdlib.h>
+#include "linmath.h"
+#include "torus_localizer.h"
+
+#define PTS 32
+#define MAX_CHECKS 40000
+#define MIN_HITS_FOR_VALID 10
+
+FLT hmd_points[PTS * 3];
+FLT hmd_norms[PTS * 3];
+
+// index for a given sensor is (2*sensor + is_sensor_y ? 1 : 0)
+FLT hmd_point_angles[PTS * 2];  
+int hmd_point_counts[PTS * 2];
+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
+            );
+    }
+}
+
+static void runTheNumbers()
+{
+    TrackedObject *to;
+
+    to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor)));
+
+    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++;
+        }
+    }
+
+    to->numSensors = sensorCount;
+
+    printf("Using %d sensors to find lighthouse.\n", sensorCount);
+
+    Point lh = SolveForLighthouse(to, 0);
+
+    printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z);
+
+    //printTrackedObject(to);
+    free(to);
+}
+
+
+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;
+}
+
+
+int main(int argc, char ** argv)
+{
+    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;
+
+
+    runTheNumbers();
+
+    return 0;
+}
diff --git a/tools/lighthousefind_tori/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h
new file mode 100644
index 0000000..a6820b5
--- /dev/null
+++ b/tools/lighthousefind_tori/tori_includes.h
@@ -0,0 +1,70 @@
+#ifndef __TORI_INCLUDES_H
+#define __TORI_INCLUDES_H
+
+#include <stddef.h>
+#include <math.h>
+#include <stdint.h>
+
+
+typedef struct
+{
+    double x;
+    double y;
+    double z;
+} Point;
+
+typedef struct
+{
+    Point point; // location of the sensor on the tracked object;
+    Point normal; // unit vector indicating the normal for the sensor
+    double theta; // "horizontal" angular measurement from lighthouse radians
+    double phi; // "vertical" angular measurement from lighthouse in radians.
+} TrackedSensor;
+
+typedef struct
+{
+    size_t numSensors;
+    TrackedSensor sensor[0];
+} TrackedObject;
+
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846264338327
+#endif
+
+#define SQUARED(x) ((x)*(x))
+
+typedef union
+{
+    struct
+    {
+        unsigned char Blue;
+        unsigned char Green;
+        unsigned char Red;
+        unsigned char Alpha;
+    };
+//    float float_value;
+    uint32_t long_value;
+} RGBValue;
+
+static RGBValue RED = { .Red = 255, .Green = 0, .Blue = 0, .Alpha = 125 };
+static RGBValue GREEN = { .Red = 0, .Green = 255, .Blue = 0, .Alpha = 125 };
+static RGBValue BLUE = { .Red = 0, .Green = 0, .Blue = 255, .Alpha = 125 };
+
+static const double WORLD_BOUNDS = 100;
+#define MAX_TRACKED_POINTS 40
+
+static const float DefaultPointsPerOuterDiameter = 60;
+
+
+
+typedef struct
+{
+    // row, column, (0,0) in upper left
+    double val[3][3];
+} Matrix3x3;
+
+
+//#define TORI_DEBUG
+
+#endif
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
new file mode 100644
index 0000000..58e4938
--- /dev/null
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -0,0 +1,561 @@
+#include <memory.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <stdio.h>
+#include "tori_includes.h"
+#include "find_tori_math.h"
+#include "visualization.h"
+
+
+Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
+{
+    Matrix3x3 result;
+    double v1[3] = { 0, 0, 1 };
+    double v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z };
+
+    normalize_v3(v2);
+
+    rotation_between_vecs_to_mat3(result.val, v1, v2);
+
+    return result;
+}
+
+Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin)
+{
+    Point q;
+
+    double pf[3] = { p.x, p.y, p.z };
+    //float pq[3];
+
+    //q.x = rot.val[0][0] * p.x + rot.val[0][1] * p.y + rot.val[0][2] * p.z + newOrigin.x;
+    //q.y = rot.val[1][0] * p.x + rot.val[1][1] * p.y + rot.val[1][2] * p.z + newOrigin.y;
+    //q.z = rot.val[2][0] * p.x + rot.val[2][1] * p.y + rot.val[2][2] * p.z + newOrigin.z;
+    q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x;
+    q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y;
+    q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z;
+
+    return q;
+}
+
+double angleFromPoints(Point p1, Point p2, Point center)
+{
+    Point v1, v2, v1norm, v2norm;
+    v1.x = p1.x - center.x;
+    v1.y = p1.y - center.y;
+    v1.z = p1.z - center.z;
+
+    v2.x = p2.x - center.x;
+    v2.y = p2.y - center.y;
+    v2.z = p2.z - center.z;
+
+    double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
+    v1norm.x = v1.x / v1mag;
+    v1norm.y = v1.y / v1mag;
+    v1norm.z = v1.z / v1mag;
+
+    double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z);
+    v2norm.x = v2.x / v2mag;
+    v2norm.y = v2.y / v2mag;
+    v2norm.z = v2.z / v2mag;
+
+    double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z;
+
+    double angle = acos(res);
+
+    return angle;
+}
+
+Point midpoint(Point a, Point b)
+{
+    Point m;
+    m.x = (a.x + b.x) / 2;
+    m.y = (a.y + b.y) / 2;
+    m.z = (a.z + b.z) / 2;
+
+    return m;
+}
+
+
+
+
+// This is the second incarnation of the torus generator.  It is intended to differ from the initial torus generator by
+// producing a point cloud of a torus where the points density is more uniform across the torus.  This will allow
+// us to be more efficient in finding a solution.
+void partialTorusGenerator(
+    Point p1,
+    Point p2,
+    double toroidalStartAngle,
+    double toroidalEndAngle,
+    double poloidalStartAngle,
+    double poloidalEndAngle,
+    double lighthouseAngle,
+    double toroidalPrecision,
+    Point **pointCloud)
+{
+    double poloidalRadius = 0;
+    double toroidalRadius = 0;
+
+    Point m = midpoint(p1, p2);
+    double distanceBetweenPoints = distance(p1, p2);
+
+    // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally
+    // thinking the tori converge at the location of the tracked object instead of at the lighthouse.
+    double centralAngleToIgnore = lighthouseAngle * 3;
+
+    Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2);
+
+    toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
+
+    poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+
+    double poloidalPrecision = M_PI * 2 / toroidalPrecision;
+
+    //unsigned int pointCount = toroidalPrecision * toroidalPrecision / 2 * (toroidalEndAngle - toroidalStartAngle) / (M_PI * 2) * (poloidalEndAngle - poloidalStartAngle) / (M_PI * 1);
+    //unsigned int pointCount = (unsigned int)(toroidalPrecision *  ((M_PI - lighthouseAngle) * 2 / poloidalPrecision + 1) + 1);
+    // TODO: This calculation of the number of points that we will generate is excessively large (probably by about a factor of 2 or more) We can do better.
+    //float pointEstimate = (pointCount + 1000) * sizeof(Point) * 2 * M_PI / (toroidalEndAngle - toroidalStartAngle);
+
+    unsigned int pointCount = 0;
+
+    for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision)
+    {
+        // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle
+        // We do this so our point cloud will have a more even distribution of points across the surface of the torus.
+        double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision;
+
+        double step_distance = 2 * M_PI / steps;
+
+        pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2);
+    }
+
+    *pointCloud = malloc(pointCount * sizeof(Point) );
+
+    assert(0 != *pointCloud);
+
+    (*pointCloud)[pointCount - 1].x = -1000;
+    (*pointCloud)[pointCount - 1].y = -1000;
+    (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now.
+
+    size_t currentPoint = 0;
+
+    for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision)
+    {
+        // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle
+        // We do this so our point cloud will have a more even distribution of points across the surface of the torus.
+        double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision;
+
+        double step_distance = 2 * M_PI / steps;
+
+        //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40)
+        for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance)
+        {
+            if (currentPoint >= pointCount - 1)
+            {
+                int a = 0;
+            }
+            assert(currentPoint < pointCount - 1);
+            (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep);
+            (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep);
+            (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep);
+            (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m);
+
+            // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up
+            // and hence we know that nothing with a negative z value is a possible lightouse location.
+            // Before this code can go live, we'll have to take the normals into account and remove this hack.
+            if ((*pointCloud)[currentPoint].z > 0)
+            {
+                currentPoint++;
+            }
+        }
+    }
+
+#ifdef TORI_DEBUG
+    printf("%d / %d\n", currentPoint, pointCount);
+#endif
+    (*pointCloud)[currentPoint].x = -1000;
+    (*pointCloud)[currentPoint].y = -1000;
+    (*pointCloud)[currentPoint].z = -1000;
+}
+
+void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointCloud)
+{
+
+    double centralAngleToIgnore = lighthouseAngle * 6;
+
+    centralAngleToIgnore = 20.0 / 180.0 * M_PI;
+
+    partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud);
+
+    return;
+}
+
+
+// What we're doing here is:
+// * Given a point in space
+// * And points and a lighthouse angle that implicitly define a torus
+// * for that torus, what is the toroidal angle of the plane that will go through that point in space
+// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space?
+//
+// Given the toroidal and poloidal angles of a "good estimate" of a solution position, a caller of this function
+// will be able to "draw" the point cloud of a torus in just the surface of the torus near the point in space.
+// That way, the caller doesn't have to draw the entire torus in high resolution, just the part of the torus
+// that is most likely to contain the best solution.
+void estimateToroidalAndPoloidalAngleOfPoint(
+    Point torusP1,
+    Point torusP2,
+    double lighthouseAngle,
+    Point point,
+    double *toroidalAngle,
+    double *poloidalAngle)
+{
+    // this is the rotation matrix that shows how to rotate the torus from being in a simple "default" orientation
+    // into the coordinate system of the tracked object
+    Matrix3x3 rot = GetRotationMatrixForTorus(torusP1, torusP2);
+
+    // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from
+    // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus.
+    // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system.
+    rot = inverseM33(rot);
+    Point origin;
+    origin.x = 0;
+    origin.y = 0;
+    origin.z = 0;
+
+    Point m = midpoint(torusP1, torusP2);
+
+    // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them
+    // This will be their representation in the "friendly" coordinate system
+    Point pointF;
+
+    // Okay, I lied a little above.  In addition to the rotation matrix that we care about, there was also
+    // a translation that we did to move the origin.  If we're going to get to the "friendly" coordinate system
+    // of the torus, we need to first undo the translation, then undo the rotation.  Below, we're undoing the translation.
+    pointF.x = point.x - m.x;
+    pointF.y = point.y - m.y;
+    pointF.z = point.z - m.z;
+
+    // now we'll undo the rotation part.
+    pointF = RotateAndTranslatePoint(pointF, rot, origin);
+
+    // hooray, now pointF is in our more-friendly coordinate system.  
+
+    // Now, it's time to figure out the toroidal angle to that point.  This should be pretty easy. 
+    // We will "flatten" the z dimension to only look at the x and y values.  Then, we just need to measure the 
+    // angle between a vector to pointF and a vector along the x axis.  
+
+    *toroidalAngle = atan(pointF.y / pointF.x);
+    if (pointF.x < 0)
+    {
+        *toroidalAngle += M_PI;
+    }
+
+    // SCORE!! We've got the toroidal angle.  We're half done!
+
+    // Okay, what next...?  Now, we will need to rotate the torus *again* to make it easy to
+    // figure out the poloidal angle.  We should rotate the entire torus by the toroidal angle
+    // so that the point we're focusin on will lie on the x/z plane.  We then should translate the
+    // torus so that the center of the poloidal circle is at the origin.  At that point, it will
+    // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a 
+    // vector from the origin to the point.
+
+    // okay, instead of rotating the torus & point by the toroidal angle to get the point on
+    // the xz plane, we're going to take advantage of the radial symmetry of the torus
+    // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't 
+    // change the torus at all).  Therefore, we'll leave the torus as is, but we'll rotate the point
+    // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent
+    // this new coordinate system
+
+    Point pointG;
+    pointG.z = pointF.z;
+    pointG.y = 0;
+    pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y));
+
+    // okay, that ended up being easier than I expected.  Now that we have the point on the xZ plane,
+    // our next step will be to shift it down so that the center of the poloidal circle is at the origin.
+    // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat
+    // this as a 2D problem.  I think we're getting close...
+
+    // I stole these lines from the torus generator.  Gonna need the poloidal radius.
+    double distanceBetweenPoints = distance(torusP1, torusP2); // we don't care about the coordinate system of these points because we're just getting distance.
+    double toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
+    double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+
+    // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. 
+    // The shift along the X axis will be the toroidal radius.  
+
+    Point pointH;
+    pointH.z = pointG.z;
+    pointH.y = pointG.y;
+    pointH.x = pointG.x - toroidalRadius;
+
+    // Okay, almost there.  If we treat pointH as a vector on the XZ plane, if we get its angle,
+    // that will be the poloidal angle we're looking for.  (crosses fingers)
+
+    *poloidalAngle = atan(pointH.z / pointH.x);
+    if (pointH.x < 0)
+    {
+        *poloidalAngle += M_PI;
+    }
+
+    // Wow, that ended up being not so much code, but a lot of interesting trig.
+    // can't remember the last time I spent so much time working through each line of code.
+
+    return;
+}
+
+double FindSmallestDistance(Point p, Point* cloud)
+{
+    Point *cp = cloud;
+    double smallestDistance = 10000000000000.0;
+
+    while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000)
+    {
+        double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z));
+        if (distance < smallestDistance)
+        {
+            smallestDistance = distance;
+        }
+        cp++;
+    }
+    smallestDistance = sqrt(smallestDistance);
+    return smallestDistance;
+}
+
+// Given a cloud and a list of clouds, find the point on masterCloud that best matches clouds.
+Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds)
+{
+
+    Point bestMatch = { 0 };
+    double bestDistance = 10000000000000.0;
+    Point *cp = masterCloud;
+    int point = 0;
+    while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000)
+    {
+        point++;
+#ifdef TORI_DEBUG
+        if (point % 100 == 0)
+        {
+            printf(".");
+        }
+#endif
+        double currentDistance = 0;
+        for (int i = 0; i < numClouds; i++)
+        {
+            if (clouds[i] == masterCloud)
+            {
+                continue;
+            }
+            Point* cloud = clouds[i];
+            currentDistance += FindSmallestDistance(*cp, cloud);
+        }
+
+        if (currentDistance < bestDistance)
+        {
+            bestDistance = currentDistance;
+            bestMatch = *cp;
+        }
+        cp++;
+    }
+
+    return bestMatch;
+}
+
+
+#define MAX_POINT_PAIRS 100
+
+typedef struct
+{
+    Point a;
+    Point b;
+    double angle;
+} PointsAndAngle;
+
+double angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
+{
+    double angle = acos(cos(a->phi - b->phi)*cos(a->theta - b->theta));
+    double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta));
+
+    return angle;
+}
+double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
+{
+    double p = (a->phi - b->phi);
+    double d = (a->theta - b->theta);
+
+    double adjd = sin((a->phi + b->phi) / 2);
+    double adjP = sin((a->theta + b->theta) / 2);
+    double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
+    return pythAngle;
+}
+Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
+{
+    PointsAndAngle pna[MAX_POINT_PAIRS];
+    //Point lh = { 10, 0, 200 };
+
+    size_t pnaCount = 0;
+    for (unsigned int i = 0; i < obj->numSensors; i++)
+    {
+        for (unsigned int j = 0; j < i; j++)
+        {
+            if (pnaCount < MAX_POINT_PAIRS)
+            {
+                pna[pnaCount].a = obj->sensor[i].point;
+                pna[pnaCount].b = obj->sensor[j].point;
+
+                pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+
+                double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
+
+                //double tmp = angleFromPoints(pna[pnaCount].a, pna[pnaCount].b, lh);
+
+                pnaCount++;
+            }
+        }
+    }
+
+    //Point **pointCloud = malloc(sizeof(Point*)* pnaCount);
+    Point **pointCloud = malloc(sizeof(void*)* pnaCount);
+
+    FILE *f = NULL;
+    if (doLogOutput)
+    {
+        f = fopen("pointcloud2.pcd", "wb");
+        writePcdHeader(f);
+        writeAxes(f);
+    }
+
+    for (unsigned int i = 0; i < pnaCount; i++)
+    {
+        torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i]));
+        if (doLogOutput)
+        {
+            writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]);
+        }
+
+    }
+
+    Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount);
+
+    if (doLogOutput)
+    {
+        markPointWithStar(f, bestMatchA, 0xFF0000);
+    }
+#ifdef TORI_DEBUG
+    printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z);
+#endif
+    // Now, let's add an extra patch or torus near the point we just found.
+
+    double toroidalAngle = 0;
+    double poloidalAngle = 0;
+
+
+
+    Point **pointCloud2 = malloc(sizeof(void*)* pnaCount);
+
+    for (unsigned int i = 0; i < pnaCount; i++)
+    {
+        estimateToroidalAndPoloidalAngleOfPoint(
+            pna[i].a,
+            pna[i].b,
+            pna[i].angle,
+            bestMatchA,
+            &toroidalAngle,
+            &poloidalAngle);
+
+        partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.2, toroidalAngle + 0.2, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i]));
+
+        if (doLogOutput)
+        {
+            writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]);
+        }
+
+    }
+
+    Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount);
+    if (doLogOutput)
+    {
+        markPointWithStar(f, bestMatchB, 0x00FF00);
+    }
+#ifdef TORI_DEBUG
+    printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z);
+#endif
+
+    Point **pointCloud3 = malloc(sizeof(void*)* pnaCount);
+
+    for (unsigned int i = 0; i < pnaCount; i++)
+    {
+        estimateToroidalAndPoloidalAngleOfPoint(
+            pna[i].a,
+            pna[i].b,
+            pna[i].angle,
+            bestMatchB,
+            &toroidalAngle,
+            &poloidalAngle);
+
+        partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.05, poloidalAngle + 0.05, pna[i].angle, 3000, &(pointCloud3[i]));
+
+        if (doLogOutput)
+        {
+            writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]);
+        }
+
+    }
+
+    Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount);
+    if (doLogOutput)
+    {
+        markPointWithStar(f, bestMatchC, 0xFFFFFF);
+    }
+#ifdef TORI_DEBUG
+    printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z);
+#endif
+
+
+    if (doLogOutput)
+    {
+        updateHeader(f);
+        fclose(f);
+    }
+
+    return bestMatchC;
+}
+
+static Point makeUnitPoint(Point *p)
+{
+    Point newP;
+    double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z);
+    newP.x = p->x / r;
+    newP.y = p->y / r;
+    newP.z = p->z / r;
+
+    return newP;
+}
+
+static double getPhi(Point p)
+{
+    //    double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z)));
+    //    double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z);
+    double phi = atan(p.x / p.z);
+    return phi;
+}
+
+static double getTheta(Point p)
+{
+    //double theta = atan(p.y / p.x);
+    double theta = atan(p.x / p.y);
+    return theta;
+}
+
+// subtraction
+static Point PointSub(Point a, Point b)
+{
+    Point newPoint;
+
+    newPoint.x = a.x - b.x;
+    newPoint.y = a.y - b.y;
+    newPoint.z = a.z - b.z;
+
+    return newPoint;
+}
+
+
diff --git a/tools/lighthousefind_tori/torus_localizer.h b/tools/lighthousefind_tori/torus_localizer.h
new file mode 100644
index 0000000..b8e7360
--- /dev/null
+++ b/tools/lighthousefind_tori/torus_localizer.h
@@ -0,0 +1,12 @@
+#ifndef __TORUS_LOCALIZER_H
+#define __TORUS_LOCALIZER_H
+#include <stdio.h>
+
+#include "tori_includes.h"
+#include "find_tori_math.h"
+
+Point SolveForLighthouse(TrackedObject *obj, char doLogOutput);
+
+
+
+#endif
diff --git a/tools/lighthousefind_tori/visualization.c b/tools/lighthousefind_tori/visualization.c
new file mode 100644
index 0000000..d348c2d
--- /dev/null
+++ b/tools/lighthousefind_tori/visualization.c
@@ -0,0 +1,94 @@
+#include "visualization.h"
+#include "tori_includes.h"
+
+int pointsWritten = 0;
+
+
+void writePoint(FILE *file, double x, double y, double z, unsigned int rgb)
+{
+    fprintf(file, "%f %f %f %u\n", x, y, z, rgb);
+    pointsWritten++;
+}
+
+void updateHeader(FILE * file)
+{
+    fseek(file, 0x4C, SEEK_SET);
+    fprintf(file, "%d", pointsWritten);
+    fseek(file, 0x7C, SEEK_SET);
+    fprintf(file, "%d", pointsWritten);
+}
+void writeAxes(FILE * file)
+{
+    double scale = 5;
+    for (double i = 0; i < scale; i = i + scale / 1000)
+    {
+        writePoint(file, i, 0, 0, 255);
+    }
+    for (double i = 0; i < scale; i = i + scale / 1000)
+    {
+        if ((int)(i / (scale / 5)) % 2 == 1)
+        {
+            writePoint(file, 0, i, 0, 255 << 8);
+        }
+    }
+    for (double i = 0; i < scale; i = i + scale / 10001)
+    {
+        if ((int)(i / (scale / 10)) % 2 == 1)
+        {
+            writePoint(file, 0, 0, i, 255 << 16);
+        }
+    }
+}
+
+void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color)
+{
+    int max = 50;
+    for (int i = 0; i < max; i++)
+    {
+        writePoint(file,
+            (a.x*i + b.x*(max - i)) / max,
+            (a.y*i + b.y*(max - i)) / max,
+            (a.z*i + b.z*(max - i)) / max,
+            color);
+    }
+}
+
+void writePcdHeader(FILE * file)
+{
+    fprintf(file, "VERSION 0.7\n");
+    fprintf(file, "FIELDS  x y z rgb\n");
+    fprintf(file, "SIZE 4 4 4 4\n");
+    fprintf(file, "TYPE F F F U\n");
+    fprintf(file, "COUNT 1 1 1 1\n");
+    fprintf(file, "WIDTH        \n");
+    fprintf(file, "HEIGHT 1\n");
+    fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n");
+    fprintf(file, "POINTS        \n");
+    fprintf(file, "DATA ascii\n");
+
+    //fprintf(file, "100000.0, 100000.0, 100000\n");
+
+}
+
+void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color)
+{
+    Point *currentPoint = pointCloud;
+
+    while (currentPoint->x != -1000 || currentPoint->y != -1000 || currentPoint->z != -1000)
+    {
+        writePoint(f, currentPoint->x, currentPoint->y, currentPoint->z, Color);
+        currentPoint++;
+    }
+}
+
+void markPointWithStar(FILE *file, Point point, unsigned int color)
+{
+    double i;
+    for (i = -0.8; i <= 0.8; i = i + 0.0025)
+    {
+        writePoint(file, point.x + i, point.y, point.z, color);
+        writePoint(file, point.x, point.y + i, point.z, color);
+        writePoint(file, point.x, point.y, point.z + i, color);
+    }
+
+}
diff --git a/tools/lighthousefind_tori/visualization.h b/tools/lighthousefind_tori/visualization.h
new file mode 100644
index 0000000..e7f9475
--- /dev/null
+++ b/tools/lighthousefind_tori/visualization.h
@@ -0,0 +1,48 @@
+
+#ifndef __VISUALIZATION_H
+#define __VISUALIZATION_H
+
+#include <stdio.h>
+#include "tori_includes.h"
+#include "find_tori_math.h"
+
+extern int pointsWritten;
+
+void writePoint(FILE *file, double x, double y, double z, unsigned int rgb);
+
+void updateHeader(FILE * file);
+
+void writeAxes(FILE * file);
+
+void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color);
+
+void writePcdHeader(FILE * file);
+
+void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color);
+
+void markPointWithStar(FILE *file, Point point, unsigned int color);
+
+#define MAX_COLORS 18
+static unsigned int COLORS[] = {
+    0x00FFFF,
+    0xFF00FF,
+    0xFFFF00,
+    0xFF0000,
+    0x00FF00,
+    0x0000FF,
+    0x0080FF,
+    0x8000FF,
+    0x80FF00,
+    0x00FF80,
+    0xFF0080,
+    0xFF8000,
+    0x008080,
+    0x800080,
+    0x808000,
+    0x000080,
+    0x008000,
+    0x800000
+};
+#endif
+
+