From 2580e9255b139da6ca4afa7f844150a3d71cdda5 Mon Sep 17 00:00:00 2001 From: mwturvey 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') 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