Merging math libraries

11 files changed, 255 insertions, 273 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index 60fbc21..ea44432 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;
@@ -65,7 +66,7 @@ void copy3d( FLT * out, const FLT * in )
 
 FLT magnitude3d( 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//////////////////////////////////////////
@@ -106,12 +107,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 +126,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 +136,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 +147,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]));
 }
 
 
@@ -296,13 +297,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 +312,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 +339,176 @@ 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;
+}
+
+/////////////////////////////////////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..530d291 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 );
 
@@ -64,6 +87,25 @@ 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 );
 
+// Matrix Stuff
+
+typedef struct
+{
+    // row, column, (0,0) in upper left
+    FLT val[3][3];
+} Matrix3x3;
+
+Matrix3x3 inverseM33(const Matrix3x3 mat);
+void get_orthogonal_vector(FLT out[3], const FLT in[3]);
+void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]);
+void unit_m3(FLT m[3][3]);
+FLT normalize_v3(FLT n[3]);
+void axis_angle_normalized_to_mat3_ex(
+    FLT mat[3][3],
+    const FLT axis[3],
+    const FLT angle_sin,
+    const FLT angle_cos);
+
 
 #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
index 3bdfdd6..b2dff64 100644
--- a/tools/lighthousefind_tori/Makefile
+++ b/tools/lighthousefind_tori/Makefile
@@ -1,7 +1,7 @@
-CFLAGS:=-g -O4 -DFLT=double -I../../redist -flto
+CFLAGS:=-g -O4 -DUSE_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)
+	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/find_tori_math.c b/tools/lighthousefind_tori/find_tori_math.c
deleted file mode 100644
index 9c305f3..0000000
--- a/tools/lighthousefind_tori/find_tori_math.c
+++ /dev/null
@@ -1,206 +0,0 @@
-#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
deleted file mode 100644
index a10c3fc..0000000
--- a/tools/lighthousefind_tori/find_tori_math.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#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/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h
index a6820b5..51cd04f 100644
--- a/tools/lighthousefind_tori/tori_includes.h
+++ b/tools/lighthousefind_tori/tori_includes.h
@@ -58,11 +58,6 @@ static const float DefaultPointsPerOuterDiameter = 60;
 
 
 
-typedef struct
-{
-    // row, column, (0,0) in upper left
-    double val[3][3];
-} Matrix3x3;
 
 
 //#define TORI_DEBUG
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 58e4938..f3040cd 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -2,16 +2,24 @@
 #include <stdlib.h>
 #include <assert.h>
 #include <stdio.h>
+#include "linmath.h"
 #include "tori_includes.h"
-#include "find_tori_math.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;
-    double v1[3] = { 0, 0, 1 };
-    double v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z };
+    FLT v1[3] = { 0, 0, 1 };
+    FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z };
 
     normalize_v3(v2);
 
@@ -25,11 +33,6 @@ 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;
diff --git a/tools/lighthousefind_tori/torus_localizer.h b/tools/lighthousefind_tori/torus_localizer.h
index b8e7360..a42e37d 100644
--- a/tools/lighthousefind_tori/torus_localizer.h
+++ b/tools/lighthousefind_tori/torus_localizer.h
@@ -3,7 +3,6 @@
 #include <stdio.h>
 
 #include "tori_includes.h"
-#include "find_tori_math.h"
 
 Point SolveForLighthouse(TrackedObject *obj, char doLogOutput);
 
diff --git a/tools/lighthousefind_tori/visualization.h b/tools/lighthousefind_tori/visualization.h
index e7f9475..f0263eb 100644
--- a/tools/lighthousefind_tori/visualization.h
+++ b/tools/lighthousefind_tori/visualization.h
@@ -4,7 +4,6 @@
 
 #include <stdio.h>
 #include "tori_includes.h"
-#include "find_tori_math.h"
 
 extern int pointsWritten;
 
diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c
index c2fd97a..c2fd97a 100755..100644
--- a/tools/plot_lighthouse/main.c
+++ b/tools/plot_lighthouse/main.c