From bb099f0fd084c3a2f84532e76928a4f548bf188e Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Wed, 15 Feb 2017 22:55:23 -0700
Subject: Eliminate unnecessary pow calls.

---
 tools/lighthousefind_tori/torus_localizer.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 837b745..c539fb5 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -122,7 +122,7 @@ void partialTorusGenerator(
 
 	toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
 
-	poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+	poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	double poloidalPrecision = M_PI * 2 / toroidalPrecision;
 
@@ -291,7 +291,7 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// 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));
+	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 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.  
@@ -533,7 +533,7 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 	Matrix3x3 rot = GetRotationMatrixForTorus(pna->a, pna->b);
 
 	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
-	double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
 	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
@@ -808,7 +808,7 @@ void AnalyzeToroidalImpact(
 
 	toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
 
-	poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+	poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	unsigned int pointCount = 0;
 
-- 
cgit v1.2.3


From 2927962bd7f9dcb6054d4bf642bb02b946379e25 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 16 Feb 2017 16:02:02 -0700
Subject: Optimizing find_tori, replace 3 trigs with 1 sqrt

---
 tools/lighthousefind_tori/torus_localizer.c | 44 +++++++++++++++++++++++------
 1 file changed, 35 insertions(+), 9 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index c539fb5..4e773a3 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -219,6 +219,8 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	double lighthouseAngle,
 	Point point,
 	double *toroidalAngle,
+	double *toroidalSin,
+	double *toroidalCos,
 	double *poloidalAngle)
 {
 	// this is the rotation matrix that shows how to rotate the torus from being in a simple "default" orientation
@@ -256,11 +258,23 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// 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;
-	}
+	FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x));
+
+	*toroidalSin = pointF.y / toroidalHyp;
+
+	*toroidalCos = pointF.x / toroidalHyp;
+
+	//*toroidalAngle = atan(pointF.y / pointF.x);
+	//if (pointF.x < 0)
+	//{
+	//	*toroidalAngle += M_PI;
+	//}
+
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+	//assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001);
 
 	// SCORE!! We've got the toroidal angle.  We're half done!
 
@@ -442,6 +456,8 @@ Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna,
 	double toroidalAngle = 0;
 	double poloidalAngle = 0;
 
+	double toroidalCos = 0;
+	double toroidalSin = 0;
 
 
 	Point **pointCloud2 = malloc(sizeof(void*)* pnaCount);
@@ -454,6 +470,8 @@ Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna,
 			pna[i].angle,
 			initialEstimate,
 			&toroidalAngle,
+			&toroidalSin,
+			&toroidalCos,
 			&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]));
@@ -491,6 +509,8 @@ Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna,
 			pna[i].angle,
 			bestMatchB,
 			&toroidalAngle,
+			&toroidalSin,
+			&toroidalCos,
 			&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]));
@@ -524,7 +544,7 @@ Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna,
 	return bestMatchC;
 }
 
-Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double poloidalAngle)
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double toroidalSin, double toroidalCos, double poloidalAngle)
 {
 	Point result;
 
@@ -535,8 +555,10 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
 	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
-	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
-	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
+	//result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
+	//result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
+	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
+	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
 	result.z = poloidalRadius*sin(poloidalAngle);
 	result = RotateAndTranslatePoint(result, rot, m);
 
@@ -547,6 +569,8 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 {
 
 	double toroidalAngle = 0;
+	double toroidalSin = 0;
+	double toroidalCos = 0;
 	double poloidalAngle = 0;
 
 	estimateToroidalAndPoloidalAngleOfPoint(
@@ -555,9 +579,11 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 		pna->angle,
 		pointIn,
 		&toroidalAngle,
+		&toroidalSin,
+		&toroidalCos,
 		&poloidalAngle);
 
-	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, poloidalAngle);
+	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, toroidalSin, toroidalCos, poloidalAngle);
 
 	FLT dist = distance(pointIn, torusPoint);
 
-- 
cgit v1.2.3


From 42640c64c93042e4a7c9fca6bc0003dff67020b0 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 16 Feb 2017 16:09:25 -0700
Subject: Disable debug code

---
 tools/lighthousefind_tori/torus_localizer.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 4e773a3..23b8dd9 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -26,8 +26,8 @@ Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
 	rotation_between_vecs_to_m3(&result, v1, v2);
 
 	// Useful for debugging...
-	FLT v2b[3];
-	rotate_vec(v2b, v1, result);
+	//FLT v2b[3];
+	//rotate_vec(v2b, v1, result);
 
 	return result;
 }
-- 
cgit v1.2.3


From 1f394751d17d14769d93ede99d3f5964984e7834 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 16 Feb 2017 16:23:22 -0700
Subject: find_tori: Cache rotation matrices

---
 tools/lighthousefind_tori/torus_localizer.c | 44 +++++++++++++----------------
 1 file changed, 20 insertions(+), 24 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 23b8dd9..f69d6ab 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -42,6 +42,15 @@ Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
 //
 //}
 
+typedef struct
+{
+	Point a;
+	Point b;
+	double angle;
+	Matrix3x3 rotation;
+} PointsAndAngle;
+
+
 Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin)
 {
 	Point q;
@@ -214,9 +223,7 @@ void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointClo
 // 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,
+	PointsAndAngle *pna,
 	Point point,
 	double *toroidalAngle,
 	double *toroidalSin,
@@ -225,7 +232,7 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 {
 	// 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);
+	Matrix3x3 rot = pna->rotation;
 
 	// 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.
@@ -236,7 +243,7 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	origin.y = 0;
 	origin.z = 0;
 
-	Point m = midpoint(torusP1, torusP2);
+	Point m = midpoint(pna->a, pna->b);
 
 	// 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
@@ -303,8 +310,8 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// 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 distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance.
+	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
 	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	// The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. 
@@ -390,13 +397,6 @@ Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds)
 
 #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));
@@ -465,9 +465,7 @@ Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna,
 	for (unsigned int i = 0; i < pnaCount; i++)
 	{
 		estimateToroidalAndPoloidalAngleOfPoint(
-			pna[i].a,
-			pna[i].b,
-			pna[i].angle,
+			&(pna[i]),
 			initialEstimate,
 			&toroidalAngle,
 			&toroidalSin,
@@ -504,9 +502,7 @@ Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna,
 	for (unsigned int i = 0; i < pnaCount; i++)
 	{
 		estimateToroidalAndPoloidalAngleOfPoint(
-			pna[i].a,
-			pna[i].b,
-			pna[i].angle,
+			&(pna[i]),
 			bestMatchB,
 			&toroidalAngle,
 			&toroidalSin,
@@ -550,7 +546,7 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 
 	double distanceBetweenPoints = distance(pna->a, pna->b);
 	Point m = midpoint(pna->a, pna->b);
-	Matrix3x3 rot = GetRotationMatrixForTorus(pna->a, pna->b);
+	Matrix3x3 rot = pna->rotation;
 
 	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
 	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
@@ -574,9 +570,7 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 	double poloidalAngle = 0;
 
 	estimateToroidalAndPoloidalAngleOfPoint(
-		pna->a,
-		pna->b,
-		pna->angle,
+		pna,
 		pointIn,
 		&toroidalAngle,
 		&toroidalSin,
@@ -922,6 +916,8 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
 
+				pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b);
+
 				pnaCount++;
 			}
 		}
-- 
cgit v1.2.3


From 243f3bea242c82d14bc2e191421010f300114c9f Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 17 Feb 2017 10:13:29 -0700
Subject: Remove a bunch of dead code

---
 tools/lighthousefind_tori/torus_localizer.c | 513 +---------------------------
 1 file changed, 10 insertions(+), 503 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index f69d6ab..25a5e7f 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -32,16 +32,6 @@ Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
 	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 }
-//
-//}
-
 typedef struct
 {
 	Point a;
@@ -101,117 +91,6 @@ Point midpoint(Point a, Point b)
 	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(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 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
@@ -337,64 +216,6 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	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
 
 double angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
@@ -415,131 +236,6 @@ double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
 	return pythAngle;
 }
 
-Point GetInitialEstimate(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
-{
-	Point **pointCloud = malloc(sizeof(void*)* pnaCount);
-
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i]));
-		if (logFile)
-		{
-			writePointCloud(logFile, 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 (logFile)
-	{
-		markPointWithStar(logFile, bestMatchA, 0xFF0000);
-	}
-#ifdef TORI_DEBUG
-	printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z);
-#endif
-
-	return bestMatchA;
-}
-
-Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
-{
-	// Now, let's add an extra patch or torus near the point we just found.
-
-	double toroidalAngle = 0;
-	double poloidalAngle = 0;
-
-	double toroidalCos = 0;
-	double toroidalSin = 0;
-
-
-	Point **pointCloud2 = malloc(sizeof(void*)* pnaCount);
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		estimateToroidalAndPoloidalAngleOfPoint(
-			&(pna[i]),
-			initialEstimate,
-			&toroidalAngle,
-			&toroidalSin,
-			&toroidalCos,
-			&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 (logFile)
-		{
-			writePointCloud(logFile, 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 (logFile)
-	{
-		markPointWithStar(logFile, 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]),
-			bestMatchB,
-			&toroidalAngle,
-			&toroidalSin,
-			&toroidalCos,
-			&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 (logFile)
-		{
-			writePointCloud(logFile, 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 (logFile)
-	{
-		markPointWithStar(logFile, bestMatchC, 0xFFFFFF);
-	}
-#ifdef TORI_DEBUG
-	printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z);
-#endif
-
-
-
-
-	return bestMatchC;
-}
-
 Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double toroidalSin, double toroidalCos, double poloidalAngle)
 {
 	Point result;
@@ -551,8 +247,6 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
 	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
-	//result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
-	//result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
 	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
 	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
 	result.z = poloidalRadius*sin(poloidalAngle);
@@ -591,14 +285,11 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 	// it is not clear if this is actually removing existing skew (to get a more accurate value)
 	// or if it is introducing an undesirable skew.
 	double fudge = FLT_SIN((poloidalAngle - M_PI) / 2);
-	//fudge *= fudge;
 	dist = dist / fudge;
 
 	return dist;
 }
 
-//Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
-//
 FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount)
 {
 	FLT fitness;
@@ -664,55 +355,6 @@ Point getAvgPoints(Point a, Point b)
 	return result;
 }
 
-// 0.95 is good value for descent
-// 0.1 is a good value for starting precision.
-static Point RefineEstimateUsingGradientDescent(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile, FLT descent, FLT startingPrecision)
-{
-	int i = 0;
-	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
-	Point lastPoint = initialEstimate;
-	Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
-
-	for (FLT f = startingPrecision; f > 0.0001; f *= descent)
-	{
-		Point gradient = getGradient(lastPoint, pna, pnaCount, f / 1000 /*somewhat arbitrary*/);
-		gradient = getNormalizedVector(gradient, f);
-
-		//printf("Gradient: (%f, %f, %f)\n", gradient.x, gradient.y, gradient.z);
-
-		// gradient = getAvgPoints(gradient, lastGradient); // doesn't seem to help much.  might hurt in some cases.
-
-		Point newPoint;
-		newPoint.x = lastPoint.x + gradient.x;
-		newPoint.y = lastPoint.y + gradient.y;
-		newPoint.z = lastPoint.z + gradient.z;
-
-		FLT newMatchFitness = getPointFitness(newPoint, pna, pnaCount);
-
-		if (newMatchFitness > lastMatchFitness)
-		{
-			lastMatchFitness = newMatchFitness;
-			lastPoint = newPoint;
-			//printf("%f\n", newMatchFitness);
-			lastGradient = gradient;
-
-			if (logFile)
-			{
-				writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
-			}
-		}
-		else
-		{
-			//printf("-");
-		}
-
-		i++;
-	}
-
-	//printf("i = %d\n", i);
-
-	return lastPoint;
-}
 
 // This is modifies the basic gradient descent algorithm to better handle the shallow valley case,
 // which appears to be typical of this convergence.  
@@ -721,7 +363,6 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 	int i = 0;
 	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
 	Point lastPoint = initialEstimate;
-	//Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
 
 	// The values below are somewhat magic, and definitely tunable
 	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
@@ -788,11 +429,15 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 
 			lastMatchFitness = newMatchFitness;
 			lastPoint = point4;
+#ifdef TORI_DEBUG
 			printf("+");
+#endif
 		}
 		else
 		{
+#ifdef TORI_DEBUG
 			printf("-");
+#endif
 			g *= 0.7;
 
 		}
@@ -804,97 +449,6 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 	return lastPoint;
 }
 
-// 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 AnalyzeToroidalImpact(
-	Point p1,
-	Point p2,
-	double lighthouseAngle,
-	double *vals,
-	PointsAndAngle *pna, 
-	size_t pnaCount)
-{
-	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(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
-
-	unsigned int pointCount = 0;
-
-	size_t currentPoint = 0;
-	
-	for (size_t ps = 0; ps < 180; ps++)
-	{
-
-		//for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40)
-		for (double toroidalStep = 0; toroidalStep < M_PI / 2; toroidalStep += M_PI / 180 * 2)
-		{
-			double poloidalStep = M_PI + M_PI / 180 * 2 * ps;
-			Point tmp;
-
-			tmp.x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep);
-			tmp.y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep);
-			tmp.z = poloidalRadius*sin(poloidalStep);
-			tmp = RotateAndTranslatePoint(tmp, rot, m);
-
-			vals[ps] += getPointFitness(tmp, pna, pnaCount);
-
-		}
-
-		vals[ps] = vals[ps] / 180; // average.
-	}
-
-}
-
-void AnalyzePoloidalImpact(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
-{
-	Point **pointCloud = malloc(sizeof(void*)* pnaCount);
-
-	double vals[200][180] = { 0 };
-
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		//double tmpVals[180] = { 0 };
-
-		AnalyzeToroidalImpact(
-			pna[i].a,
-			pna[i].b,
-			pna[i].angle,
-			vals[i],
-			pna,
-			pnaCount);
-
-
-		//for (int j = 0; j < 180; j++)
-		//{
-		//	vals[j] += tmpVals[j];
-		//}
-
-	}
-	
-	for (int i = 0; i < 180; i++)
-	{
-		printf("%d", i * 2);
-		for (unsigned int j = 0; j < pnaCount; j++)
-		{
-			printf(", %f", vals[j][i]);
-		}
-		printf("\n");
-	}
-}
-
 
 Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
@@ -941,15 +495,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		writeAxes(logFile);
 	}
 
-	//Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile);
-
-	//Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile);
-
-	//Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
 
 	// Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
 
-	// AnalyzePoloidalImpact(obj, pna, pnaCount, logFile);
 
 	// arbitrarily picking a value of 8 meters out to start from.
 	// intentionally picking the direction of the average normal vector of the sensors that see the lighthouse
@@ -957,16 +505,16 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 	// back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
 	Point p1 = getNormalizedVector(avgNorm, 8); 
 
-	Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
+	Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
 
-	FLT pf1[3] = { refinedEstimageGd.x, refinedEstimageGd.y, refinedEstimageGd.z };
+	FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
 
 	FLT a1 = anglebetween3d(pf1, avgNormF);
 
 	if (a1 > M_PI / 2)
 	{
-		Point p2 = { .x = -refinedEstimageGd.x, .y = -refinedEstimageGd.y, .z = -refinedEstimageGd.z };
-		refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
+		Point p2 = { .x = -refinedEstimateGd.x, .y = -refinedEstimateGd.y, .z = -refinedEstimateGd.z };
+		refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
 
 		//FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
 
@@ -974,9 +522,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 	}
 
-	//FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
-	FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
-	//FLT fitGd2 = getPointFitness(refinedEstimageGd2, pna, pnaCount);
+	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount);
 
 	printf("Fitness is %f\n", fitGd);
 
@@ -986,45 +532,6 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		fclose(logFile);
 	}
 	//fgetc(stdin);
-	return refinedEstimageGd;
+	return refinedEstimateGd;
 }
 
-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;
-}
-
-
-- 
cgit v1.2.3


From 2e10182d0050cceb38d8bd247ca8a0ca6af4ae87 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 17 Feb 2017 10:37:30 -0700
Subject: find_tori: Cache inverse rotation & trade a sin for a sqrt

---
 tools/lighthousefind_tori/torus_localizer.c | 33 +++++++++++++++++++----------
 1 file changed, 22 insertions(+), 11 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 25a5e7f..17a1b5a 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -38,6 +38,7 @@ typedef struct
 	Point b;
 	double angle;
 	Matrix3x3 rotation;
+	Matrix3x3 invRotation;
 } PointsAndAngle;
 
 
@@ -107,16 +108,13 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	double *toroidalAngle,
 	double *toroidalSin,
 	double *toroidalCos,
-	double *poloidalAngle)
+	double *poloidalAngle,
+	double *poloidalSin)
 {
-	// 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 = pna->rotation;
-
 	// 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);
+	Matrix3x3 rot = pna->invRotation;
 	Point origin;
 	origin.x = 0;
 	origin.y = 0;
@@ -204,12 +202,22 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// 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)
 
+	FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x));
+
+	*poloidalSin = pointH.z / poloidalHyp;
+
+
 	*poloidalAngle = atan(pointH.z / pointH.x);
 	if (pointH.x < 0)
 	{
 		*poloidalAngle += M_PI;
 	}
 
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+
+
 	// 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.
 
@@ -236,7 +244,7 @@ double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
 	return pythAngle;
 }
 
-Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double toroidalSin, double toroidalCos, double poloidalAngle)
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double toroidalSin, double toroidalCos, double poloidalAngle, double poloidalSin)
 {
 	Point result;
 
@@ -249,7 +257,7 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 
 	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
 	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
-	result.z = poloidalRadius*sin(poloidalAngle);
+	result.z = poloidalRadius*poloidalSin;
 	result = RotateAndTranslatePoint(result, rot, m);
 
 	return result;
@@ -262,6 +270,7 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 	double toroidalSin = 0;
 	double toroidalCos = 0;
 	double poloidalAngle = 0;
+	double poloidalSin = 0;
 
 	estimateToroidalAndPoloidalAngleOfPoint(
 		pna,
@@ -269,9 +278,10 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 		&toroidalAngle,
 		&toroidalSin,
 		&toroidalCos,
-		&poloidalAngle);
+		&poloidalAngle,
+		&poloidalSin);
 
-	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, toroidalSin, toroidalCos, poloidalAngle);
+	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, toroidalSin, toroidalCos, poloidalAngle, poloidalSin);
 
 	FLT dist = distance(pointIn, torusPoint);
 
@@ -299,7 +309,6 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount)
 	for (size_t i = 0; i < pnaCount; i++)
 	{
 		fitness = getPointFitnessForPna(pointIn, &(pna[i]));
-		//printf("Distance[%d]: %f\n", i, fitness);
 		resultSum += SQUARED(fitness);
 	}
 
@@ -471,6 +480,8 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
 
 				pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b);
+				pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation);
+
 
 				pnaCount++;
 			}
-- 
cgit v1.2.3


From 2580e9255b139da6ca4afa7f844150a3d71cdda5 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 17 Feb 2017 10:53:39 -0700
Subject: find_tori: Cache a tan()

---
 tools/lighthousefind_tori/torus_localizer.c | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 17a1b5a..baf9d47 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -36,9 +36,11 @@ typedef struct
 {
 	Point a;
 	Point b;
-	double angle;
+	FLT angle;
+	FLT tanAngle; // tangent of angle
 	Matrix3x3 rotation;
-	Matrix3x3 invRotation;
+	Matrix3x3 invRotation; // inverse of rotation
+
 } PointsAndAngle;
 
 
@@ -188,7 +190,7 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 
 	// I stole these lines from the torus generator.  Gonna need the poloidal radius.
 	double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance.
-	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
+	double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
 	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	// The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. 
@@ -252,7 +254,7 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 	Point m = midpoint(pna->a, pna->b);
 	Matrix3x3 rot = pna->rotation;
 
-	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
+	double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
 	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
@@ -463,6 +465,8 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
 	PointsAndAngle pna[MAX_POINT_PAIRS];
 
+	volatile size_t sizeNeeded = sizeof(pna);
+
 	Point avgNorm = { 0 };
 
 	size_t pnaCount = 0;
@@ -476,6 +480,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 				pna[pnaCount].b = obj->sensor[j].point;
 
 				pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+				pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle);
 
 				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
 
-- 
cgit v1.2.3


From 8e7ddd9c728f43aa1189f3add99adbb3ba8a74f1 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Tue, 21 Feb 2017 13:24:02 -0700
Subject: Use a better angle estimate

---
 tools/lighthousefind_tori/torus_localizer.c | 45 ++++++++++++++---------------
 1 file changed, 21 insertions(+), 24 deletions(-)

(limited to 'tools/lighthousefind_tori/torus_localizer.c')

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index baf9d47..fd74b22 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -99,15 +99,9 @@ Point midpoint(Point a, Point b)
 // * 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(
 	PointsAndAngle *pna,
 	Point point,
-	double *toroidalAngle,
 	double *toroidalSin,
 	double *toroidalCos,
 	double *poloidalAngle,
@@ -228,34 +222,38 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 
 #define MAX_POINT_PAIRS 100
 
-double angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
+FLT 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));
+	FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta));
+	FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
 
 	return angle;
 }
-double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
+
+// This provides a pretty good estimate of the angle above, probably better
+// the further away the lighthouse is.  But, it's not crazy-precise.
+// It's main advantage is speed.
+FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
 {
-	double p = (a->phi - b->phi);
-	double d = (a->theta - b->theta);
+	FLT p = (a->phi - b->phi);
+	FLT 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));
+	FLT adjd = FLT_SIN((a->phi + b->phi) / 2);
+	FLT adjP = FLT_SIN((a->theta + b->theta) / 2);
+	FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
 	return pythAngle;
 }
 
-Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double toroidalSin, double toroidalCos, double poloidalAngle, double poloidalSin)
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin)
 {
 	Point result;
 
-	double distanceBetweenPoints = distance(pna->a, pna->b);
+	FLT distanceBetweenPoints = distance(pna->a, pna->b);
 	Point m = midpoint(pna->a, pna->b);
 	Matrix3x3 rot = pna->rotation;
 
-	double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
-	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
+	FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+	FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
 	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
@@ -268,7 +266,6 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 {
 
-	double toroidalAngle = 0;
 	double toroidalSin = 0;
 	double toroidalCos = 0;
 	double poloidalAngle = 0;
@@ -277,13 +274,12 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 	estimateToroidalAndPoloidalAngleOfPoint(
 		pna,
 		pointIn,
-		&toroidalAngle,
 		&toroidalSin,
 		&toroidalCos,
 		&poloidalAngle,
 		&poloidalSin);
 
-	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, toroidalSin, toroidalCos, poloidalAngle, poloidalSin);
+	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin);
 
 	FLT dist = distance(pointIn, torusPoint);
 
@@ -478,8 +474,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 			{
 				pna[pnaCount].a = obj->sensor[i].point;
 				pna[pnaCount].b = obj->sensor[j].point;
-
-				pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+				
+				pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]);
+				//pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
 				pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle);
 
 				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
-- 
cgit v1.2.3