Merge branch 'master' of https://github.com/cnlohr/libsurvive

12 files changed, 1427 insertions, 62 deletions

diff --git a/README.md b/README.md
index bb7e520..f90590f 100644
--- a/README.md
+++ b/README.md
@@ -4,13 +4,16 @@
 
 Discord: https://discordapp.com/invite/7QbCAGS
 
-First livestream: https://www.youtube.com/watch?v=sv_AVI9kHN4
-
-Second livestream: https://www.youtube.com/watch?v=gFyEbGQ88s4
-
-First summary livestream: https://www.youtube.com/watch?v=oHJkpNakswM
-
-Third livestream: https://www.youtube.com/watch?v=RExji5EtSzE
+## Livestream collection
+| Note                                   | Youtube URL                                 | Run time |
+| -------------------------------------- | ------------------------------------------- | -------- |
+| First livestream                       | https://www.youtube.com/watch?v=sv_AVI9kHN4 | 5:01:25  |
+| Second livestream                      | https://www.youtube.com/watch?v=gFyEbGQ88s4 | 4:03:26  |
+| Summary of first and second livestream | https://www.youtube.com/watch?v=oHJkpNakswM | 23:00   |
+| Third livestream                       | https://www.youtube.com/watch?v=RExji5EtSzE | 4:11:16 |
+| Fourth livestream                      | https://www.youtube.com/watch?v=fces1O7kWGY | 4:50:33 |
+| Fifth livestream                       | https://www.youtube.com/watch?v=hHt3twW5_fI | 3:13:38 |
+| Sixth livestream                       | https://www.youtube.com/watch?v=JsfkNRFkFM4 | 3:44:49 |
 
 Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e
 
diff --git a/redist/linmath.c b/redist/linmath.c
index 60fbc21..d5d54e3 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -2,6 +2,7 @@
 
 #include "linmath.h"
 #include <math.h>
+#include <float.h>
 
 void cross3d( FLT * out, const FLT * a, const FLT * b )
 {
@@ -33,7 +34,7 @@ void scale3d( FLT * out, const FLT * a, FLT scalar )
 
 void normalize3d( FLT * out, const FLT * in )
 {
-	FLT r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] );
+	FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);
 	out[0] = in[0] * r;
 	out[1] = in[1] * r;
 	out[2] = in[2] * r;
@@ -63,9 +64,9 @@ void copy3d( FLT * out, const FLT * in )
 	out[2] = in[2];
 }
 
-FLT magnitude3d( FLT * a )
+FLT magnitude3d(const FLT * a )
 {
-	return sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
+	return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
 }
 
 FLT anglebetween3d( FLT * a, FLT * b )
@@ -77,7 +78,7 @@ FLT anglebetween3d( FLT * a, FLT * b )
 	FLT dot = dot3d( a, b );
 	if( dot < -0.9999999 ) return LINMATHPI;
 	if( dot >  0.9999999 ) return 0;
-	return acos( dot );
+	return FLT_ACOS(dot);
 }
 
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
@@ -87,12 +88,17 @@ FLT anglebetween3d( FLT * a, FLT * b )
 
 
 
-void quatsetnone( FLT * q )
+void quatsetnone(FLT * q)
 {
 	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
 }
 
-void quatcopy( FLT * qout, const FLT * qin )
+void quatsetidentity(FLT * q)
+{
+	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1;
+}
+
+void quatcopy(FLT * qout, const FLT * qin)
 {
 	qout[0] = qin[0];
 	qout[1] = qin[1];
@@ -106,12 +112,12 @@ void quatfromeuler( FLT * q, const FLT * euler )
 	FLT Y = euler[1]/2.0f; //pitch
 	FLT Z = euler[2]/2.0f; //yaw
 
-	FLT cx = cosf(X);
-	FLT sx = sinf(X);
-	FLT cy = cosf(Y);
-	FLT sy = sinf(Y);
-	FLT cz = cosf(Z);
-	FLT sz = sinf(Z);
+	FLT cx = FLT_COS(X);
+	FLT sx = FLT_SIN(X);
+	FLT cy = FLT_COS(Y);
+	FLT sy = FLT_SIN(Y);
+	FLT cz = FLT_COS(Z);
+	FLT sz = FLT_SIN(Z);
 
 	//Correct according to
 	//http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
@@ -125,9 +131,9 @@ void quatfromeuler( FLT * q, const FLT * euler )
 void quattoeuler( FLT * euler, const FLT * q )
 {
 	//According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
-	euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) );
-	euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) );
-	euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) );
+	euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]));
+	euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1]));
+	euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]));
 }
 
 void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
@@ -135,8 +141,8 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
 	FLT v[3];
 	normalize3d( v, axis );
 	
-	FLT sn = sin(radians/2.0f);
-	q[0] = cos(radians/2.0f);
+	FLT sn = FLT_SIN(radians / 2.0f);
+	q[0] = FLT_COS(radians / 2.0f);
 	q[1] = sn * v[0];
 	q[2] = sn * v[1];
 	q[3] = sn * v[2];
@@ -146,12 +152,12 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
 
 FLT quatmagnitude( const FLT * q )
 {
-	return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+	return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
 }
 
 FLT quatinvsqmagnitude( const FLT * q )
 {
-	return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+	return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
 }
 
 
@@ -161,38 +167,38 @@ void quatnormalize( FLT * qout, const FLT * qin )
 	quatscale( qout, qin, imag );
 }
 
-void quattomatrix( FLT * matrix44, const FLT * qin )
+void quattomatrix(FLT * matrix44, const FLT * qin)
 {
 	FLT q[4];
-	quatnormalize( q, qin );
-	
+	quatnormalize(q, qin);
+
 	//Reduced calulation for speed
-	FLT xx = 2*q[0]*q[0];
-	FLT xy = 2*q[0]*q[1];
-	FLT xz = 2*q[0]*q[2];
-	FLT xw = 2*q[0]*q[3];
-	
-	FLT yy = 2*q[1]*q[1];
-	FLT yz = 2*q[1]*q[2];
-	FLT yw = 2*q[1]*q[3];
-	
-	FLT zz = 2*q[2]*q[2];
-	FLT zw = 2*q[2]*q[3];
+	FLT xx = 2 * q[0] * q[0];
+	FLT xy = 2 * q[0] * q[1];
+	FLT xz = 2 * q[0] * q[2];
+	FLT xw = 2 * q[0] * q[3];
+
+	FLT yy = 2 * q[1] * q[1];
+	FLT yz = 2 * q[1] * q[2];
+	FLT yw = 2 * q[1] * q[3];
+
+	FLT zz = 2 * q[2] * q[2];
+	FLT zw = 2 * q[2] * q[3];
 
 	//opengl major
-	matrix44[0] = 1-yy-zz;
-	matrix44[1] = xy-zw;
-	matrix44[2] = xz+yw;
+	matrix44[0] = 1 - yy - zz;
+	matrix44[1] = xy - zw;
+	matrix44[2] = xz + yw;
 	matrix44[3] = 0;
 
-	matrix44[4] = xy+zw;
-	matrix44[5] = 1-xx-zz;
-	matrix44[6] = yz-xw;
+	matrix44[4] = xy + zw;
+	matrix44[5] = 1 - xx - zz;
+	matrix44[6] = yz - xw;
 	matrix44[7] = 0;
 
-	matrix44[8] = xz-yw;
-	matrix44[9] = yz+xw;
-	matrix44[10] = 1-xx-yy;
+	matrix44[8] = xz - yw;
+	matrix44[9] = yz + xw;
+	matrix44[10] = 1 - xx - yy;
 	matrix44[11] = 0;
 
 	matrix44[12] = 0;
@@ -201,7 +207,39 @@ void quattomatrix( FLT * matrix44, const FLT * qin )
 	matrix44[15] = 1;
 }
 
-void quatgetconjugate( FLT * qout, const FLT * qin )
+void quattomatrix33(FLT * matrix33, const FLT * qin)
+{
+	FLT q[4];
+	quatnormalize(q, qin);
+
+	//Reduced calulation for speed
+	FLT xx = 2 * q[0] * q[0];
+	FLT xy = 2 * q[0] * q[1];
+	FLT xz = 2 * q[0] * q[2];
+	FLT xw = 2 * q[0] * q[3];
+
+	FLT yy = 2 * q[1] * q[1];
+	FLT yz = 2 * q[1] * q[2];
+	FLT yw = 2 * q[1] * q[3];
+
+	FLT zz = 2 * q[2] * q[2];
+	FLT zw = 2 * q[2] * q[3];
+
+	//opengl major
+	matrix33[0] = 1 - yy - zz;
+	matrix33[1] = xy - zw;
+	matrix33[2] = xz + yw;
+
+	matrix33[3] = xy + zw;
+	matrix33[4] = 1 - xx - zz;
+	matrix33[5] = yz - xw;
+
+	matrix33[6] = xz - yw;
+	matrix33[7] = yz + xw;
+	matrix33[8] = 1 - xx - yy;
+}
+
+void quatgetconjugate(FLT * qout, const FLT * qin)
 {
 	qout[0] = qin[0];
 	qout[1] = -qin[1];
@@ -296,13 +334,13 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
 	if ( 1 - (cosTheta*cosTheta) <= 0 )
 		sinTheta = 0;
 	else
-		sinTheta = sqrt(1 - (cosTheta*cosTheta));
+		sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta));
 
-	FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions
+	FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions
 
-	if(fabs(Theta) < DEFAULT_EPSILON )
+	if (FLT_FABS(Theta) < DEFAULT_EPSILON)
 		quatcopy( q, qa );
-	else if(fabs(sinTheta) < DEFAULT_EPSILON )
+	else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON)
 	{
 		quatadd( q, qa, qb );
 		quatscale( q, q, 0.5 );
@@ -311,10 +349,10 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
 	{
 		FLT aside[4];
 		FLT bside[4];
-		quatscale( bside, qb, sin( t * Theta ) );
-		quatscale( aside, qa, sin((1-t)*Theta) );
+		quatscale( bside, qb, FLT_SIN(t * Theta));
+		quatscale( aside, qa, FLT_SIN((1 - t)*Theta));
 		quatadd( q, aside, bside );
-		quatscale( q, q, 1./sinTheta );
+		quatscale( q, q, ((FLT)1.)/sinTheta );
 	}
 }
 
@@ -338,4 +376,260 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
 }
 
 
+// Matrix Stuff
+
+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++)
+		{
+			FLT tmp = newMat.val[i][j];
+			newMat.val[i][j] = newMat.val[j][i];
+			newMat.val[j][i] = tmp;
+		}
+	}
+
+	return newMat;
+}
+
+void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3])
+{
+	FLT q[4];
+
+	quatfrom2vectors(q, v1, v2);
+
+	quattomatrix33(&(m->val[0][0]), q);
+}
+
+void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot)
+{
+	out[0] = rot.val[0][0] * in[0] + rot.val[1][0] * in[1] + rot.val[2][0] * in[2];
+	out[1] = rot.val[0][1] * in[0] + rot.val[1][1] * in[1] + rot.val[2][1] * in[2];
+	out[2] = rot.val[0][2] * in[0] + rot.val[1][2] * in[1] + rot.val[2][2] * in[2];
+
+	return;
+}
+
+
+// This function based on code from Object-oriented Graphics Rendering Engine
+// Copyright(c) 2000 - 2012 Torus Knot Software Ltd
+// under MIT license
+// http://www.ogre3d.org/docs/api/1.9/_ogre_vector3_8h_source.html
+
+/** Gets the shortest arc quaternion to rotate this vector to the destination
+vector.
+@remarks
+If you call this with a dest vector that is close to the inverse
+of this vector, we will rotate 180 degrees around a generated axis if
+since in this case ANY axis of rotation is valid.
+*/
+void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
+{
+	// Based on Stan Melax's article in Game Programming Gems
+
+	// Copy, since cannot modify local
+	FLT v0[3];
+	FLT v1[3];
+	normalize3d(v0, src);
+	normalize3d(v1, dest);
+
+	FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
+	// If dot == 1, vectors are the same
+	if (d >= 1.0f)
+	{
+		quatsetidentity(q);
+		return;
+	}
+	if (d < (1e-6f - 1.0f))
+	{
+		// Generate an axis
+		FLT unitX[3] = { 1, 0, 0 };
+		FLT unitY[3] = { 0, 1, 0 };
+		
+		FLT axis[3];
+		cross3d(axis, unitX, src); // pick an angle
+		if ((axis[0] < 1.0e-35f) &&
+			(axis[1] < 1.0e-35f) &&
+			(axis[2] < 1.0e-35f)) // pick another if colinear
+		{
+			cross3d(axis, unitY, src);
+		}
+		normalize3d(axis, axis);
+		quatfromaxisangle(q, axis, LINMATHPI);
+	}
+	else
+	{
+		FLT s = FLT_SQRT((1 + d) * 2);
+		FLT invs = 1 / s;
+
+		FLT c[3];
+		//cross3d(c, v0, v1);
+		cross3d(c, v1, v0);
+
+		q[0] = c[0] * invs;
+		q[1] = c[1] * invs;
+		q[2] = c[2] * invs;
+		q[3] = s * 0.5f;
+
+		quatnormalize(q, q);
+	}
+
+}
+
+///////////////////////////////////////Matrix Rotations////////////////////////////////////
+////Originally from Stack Overflow 
+////Under cc by-sa 3.0
+////  http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw
+//// Copyright 2014 by Campbell Barton
+//// Copyright 2017 by Michael Turvey
+//
+///**
+//* Calculate a rotation matrix from 2 normalized vectors.
+//*
+//* v1 and v2 must be unit length.
+//*/
+//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3])
+//{
+//	FLT axis[3];
+//	/* avoid calculating the angle */
+//	FLT angle_sin;
+//	FLT angle_cos;
+//
+//	cross3d(axis, v1, v2);
+//
+//	angle_sin = normalize_v3(axis);
+//	angle_cos = dot3d(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... */
+//			get_orthogonal_vector(axis, v1);
+//			normalize_v3(axis);
+//			angle_sin = 0.0f;  /* sin(M_PI) */
+//			angle_cos = -1.0f;  /* cos(M_PI) */
+//			goto axis_calc;
+//		}
+//	}
+//}
+
+//void get_orthogonal_vector(FLT out[3], const FLT in[3])
+//{
+//#ifdef USE_DOUBLE
+//	const FLT x = fabs(in[0]);
+//	const FLT y = fabs(in[1]);
+//	const FLT z = fabs(in[2]);
+//#else
+//	const FLT x = fabsf(in[0]);
+//	const FLT y = fabsf(in[1]);
+//	const FLT z = fabsf(in[2]);
+//#endif
+//
+//	if (x > y && x > z)
+//	{
+//		// x is dominant
+//		out[0] = -in[1] - in[2];
+//		out[1] = in[0];
+//		out[2] = in[0];
+//	}
+//	else if (y > z)
+//	{
+//		// y is dominant
+//		out[0] = in[1];
+//		out[1] = -in[0] - in[2];
+//		out[2] = in[1];
+//	}
+//	else
+//	{
+//		// z is dominant
+//		out[0] = in[2];
+//		out[1] = in[2];
+//		out[2] = -in[0] - in[1];
+//	}
+//}
+//
+//void unit_m3(FLT mat[3][3])
+//{
+//	mat[0][0] = 1;
+//	mat[0][1] = 0;
+//	mat[0][2] = 0;
+//	mat[1][0] = 0;
+//	mat[1][1] = 1;
+//	mat[1][2] = 0;
+//	mat[2][0] = 0;
+//	mat[2][1] = 0;
+//	mat[2][2] = 1;
+//}
+
+
+//FLT normalize_v3(FLT vect[3])
+//{
+//	FLT distance = dot3d(vect, vect);
+//
+//	if (distance < 1.0e-35f)
+//	{
+//		// distance is too short, just go to zero.
+//		vect[0] = 0;
+//		vect[1] = 0;
+//		vect[2] = 0;
+//		distance = 0;
+//	}
+//	else
+//	{
+//		distance = FLT_SQRT((FLT)distance);
+//		scale3d(vect, vect, 1.0f / distance);
+//	}
+//
+//	return distance;
+//}
+
+///* axis must be unit length */
+//void axis_angle_normalized_to_mat3_ex(
+//	FLT mat[3][3], const FLT axis[3],
+//	const FLT angle_sin, const FLT angle_cos)
+//{
+//	FLT nsi[3], ico;
+//	FLT 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/redist/linmath.h b/redist/linmath.h
index 5cc7b7d..20a7848 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -10,14 +10,37 @@
 #define PFTHREE(x) x[0], x[1], x[2]
 #define PFFOUR(x) x[0], x[1], x[2], x[3]
 
-#define LINMATHPI 3.141592653589
+#define LINMATHPI ((FLT)3.141592653589)
+
+//uncomment the following line to use double precision instead of single precision.
+//#define USE_DOUBLE
+
+#ifdef USE_DOUBLE
+
+#define FLT double
+#define FLT_SQRT sqrt
+#define FLT_SIN  sin
+#define FLT_COS  cos
+#define FLT_ACOS  acos
+#define FLT_ASIN  asin
+#define FLT_ATAN2  atan2
+#define FLT_FABS fabs
+
+#else
 
-//If you want, you can define FLT to be double for double precision.
-#ifndef FLT
 #define FLT float
+#define FLT_SQRT sqrtf
+#define FLT_SIN  sinf
+#define FLT_COS  cosf
+#define FLT_ACOS  acosf
+#define FLT_ASIN  asinf
+#define FLT_ATAN2  atan2f
+#define FLT_FABS fabsf
+
 #endif
 
 
+
 //NOTE: Inputs may never be output with cross product.
 void cross3d( FLT * out, const FLT * a, const FLT * b );
 
@@ -36,7 +59,7 @@ int compare3d( const FLT * a, const FLT * b, FLT epsilon );
 
 void copy3d( FLT * out, const FLT * in );
 
-FLT magnitude3d( FLT * a );
+FLT magnitude3d(const FLT * a );
 
 FLT anglebetween3d( FLT * a, FLT * b );
 
@@ -64,6 +87,23 @@ void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb );
 void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t );
 void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
 
+void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest);
+
+// Matrix Stuff
+
+typedef struct
+{
+	FLT val[3][3]; // row, column
+} Matrix3x3;
+
+void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot);
+void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]);
+Matrix3x3 inverseM33(const Matrix3x3 mat);
+
+
+
+
+
 
 #endif
 
diff --git a/tools/lighthousefind/Makefile b/tools/lighthousefind/Makefile
index 032ade7..cb073d9 100644
--- a/tools/lighthousefind/Makefile
+++ b/tools/lighthousefind/Makefile
@@ -1,6 +1,6 @@
 all : lighthousefind
 
-CFLAGS:=-g -O4 -DFLT=double -I../../redist -flto
+CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto
 LDFLAGS:=$(CFLAGS) -lm 
 
 lighthousefind : lighthousefind.o ../../redist/linmath.c
diff --git a/tools/lighthousefind_tori/Makefile b/tools/lighthousefind_tori/Makefile
new file mode 100644
index 0000000..b2dff64
--- /dev/null
+++ b/tools/lighthousefind_tori/Makefile
@@ -0,0 +1,7 @@
+CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto
+LDFLAGS:=$(CFLAGS) -lm
+
+all:
+	gcc -O3 -o lighthousefind-tori main.c torus_localizer.c visualization.c ../../redist/linmath.c $(LDFLAGS)
+clean:
+	rm -f lighthousefind-tori
diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c
new file mode 100644
index 0000000..aa51448
--- /dev/null
+++ b/tools/lighthousefind_tori/main.c
@@ -0,0 +1,217 @@
+#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] + LINMATHPI / 2;
+			to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2;
+			sensorCount++;
+		}
+	}
+
+	to->numSensors = sensorCount;
+
+	printf("Using %d sensors to find lighthouse.\n", sensorCount);
+
+	Point lh = SolveForLighthouse(to, 1);
+
+	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] = ((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);
+	}
+
+	//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..4cfbcdc
--- /dev/null
+++ b/tools/lighthousefind_tori/tori_includes.h
@@ -0,0 +1,60 @@
+#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;
+	};
+	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;
+
+//#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..22a0ce2
--- /dev/null
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -0,0 +1,592 @@
+#include <memory.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <stdio.h>
+#include "linmath.h"
+#include "tori_includes.h"
+#include "visualization.h"
+
+
+static 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);
+}
+
+Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
+{
+	Matrix3x3 result;
+	FLT v1[3] = { 0, 0, 1 };
+	FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z };
+
+	normalize3d(v2,v2);
+
+	rotation_between_vecs_to_m3(&result, v1, v2);
+
+	// Useful for debugging...
+	FLT v2b[3];
+	rotate_vec(v2b, v1, result);
+
+	return result;
+}
+
+//void TestGetRotationMatrixForTorus()
+//{
+//	// a={x=0.079967096447944641 y=0.045225199311971664 z=0.034787099808454514 }
+//	// b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 }
+//
+//	// a={x=0.050822000950574875 y=0.052537899464368820 z=0.033285100013017654 }
+//	// b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 }
+//
+//}
+
+Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin)
+{
+	Point q;
+
+	double pf[3] = { p.x, p.y, p.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 torus generator creates a point cloud of the given torus, and attempts to keep the 
+// density of the points fairly uniform across the surface of the torus.
+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 = 0;
+
+	// This loop tries to (cheaply) figure out an upper bound on the number of points we'll have in our point cloud
+	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];
+
+	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));
+
+				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);
+
+	for (unsigned int i = 0; i < pnaCount; i++)
+	{
+		free(pointCloud[i]);
+		pointCloud[i] = NULL;
+	}
+
+	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.1, toroidalAngle + 0.1, 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);
+
+	for (unsigned int i = 0; i < pnaCount; i++)
+	{
+		free(pointCloud2[i]);
+		pointCloud2[i] = NULL;
+	}
+
+	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.1, poloidalAngle + 0.1, pna[i].angle, 3000, &(pointCloud3[i]));
+
+		if (doLogOutput)
+		{
+			writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]);
+		}
+
+	}
+
+	Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount);
+
+	for (unsigned int i = 0; i < pnaCount; i++)
+	{
+		free(pointCloud3[i]);
+		pointCloud3[i] = NULL;
+	}
+
+	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..a42e37d
--- /dev/null
+++ b/tools/lighthousefind_tori/torus_localizer.h
@@ -0,0 +1,11 @@
+#ifndef __TORUS_LOCALIZER_H
+#define __TORUS_LOCALIZER_H
+#include <stdio.h>
+
+#include "tori_includes.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..12cbfee
--- /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..da69ed2
--- /dev/null
+++ b/tools/lighthousefind_tori/visualization.h
@@ -0,0 +1,47 @@
+
+#ifndef __VISUALIZATION_H
+#define __VISUALIZATION_H
+
+#include <stdio.h>
+#include "tori_includes.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
+
+
diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c
index 8088828..8088828 100755..100644
--- a/tools/plot_lighthouse/main.c
+++ b/tools/plot_lighthouse/main.c