From 0586f134e02938e1a9dd86ad92e41c2b2443fee0 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 7 Feb 2017 17:43:12 -0700 Subject: Replacing rotation matrix (wip) --- tools/lighthousefind_tori/main.c | 2 +- tools/lighthousefind_tori/torus_localizer.c | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) (limited to 'tools') diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c index 2fd517a..14d9423 100644 --- a/tools/lighthousefind_tori/main.c +++ b/tools/lighthousefind_tori/main.c @@ -61,7 +61,7 @@ static void runTheNumbers() printf("Using %d sensors to find lighthouse.\n", sensorCount); - Point lh = SolveForLighthouse(to, 0); + Point lh = SolveForLighthouse(to, 1); printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c index f3040cd..43dd9a2 100644 --- a/tools/lighthousefind_tori/torus_localizer.c +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -25,6 +25,20 @@ Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) rotation_between_vecs_to_mat3(result.val, v1, v2); + FLT result2[9]; + + rotation_between_vecs_to_m3(result2, v1, v2); + + result.val[0][0] = result2[0]; + result.val[0][1] = result2[1]; + result.val[0][2] = result2[2]; + result.val[1][0] = result2[3]; + result.val[1][1] = result2[4]; + result.val[1][2] = result2[5]; + result.val[2][0] = result2[6]; + result.val[2][1] = result2[7]; + result.val[2][2] = result2[8]; + return result; } -- cgit v1.2.3