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