From a4cf0b14abb17c313243d0fb84555aec2cef61a0 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 7 Feb 2017 00:11:39 -0700 Subject: Merging math libraries --- tools/lighthousefind_tori/torus_localizer.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 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 58e4938..f3040cd 100644 --- a/tools/lighthousefind_tori/torus_localizer.c +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -2,16 +2,24 @@ #include #include #include +#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; -- cgit v1.2.3