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 --- redist/linmath.c | 219 +++++++++++++++++++++++++--- redist/linmath.h | 48 +++++- tools/lighthousefind/Makefile | 2 +- tools/lighthousefind_tori/Makefile | 4 +- tools/lighthousefind_tori/find_tori_math.c | 206 -------------------------- tools/lighthousefind_tori/find_tori_math.h | 23 --- tools/lighthousefind_tori/tori_includes.h | 5 - tools/lighthousefind_tori/torus_localizer.c | 19 ++- tools/lighthousefind_tori/torus_localizer.h | 1 - tools/lighthousefind_tori/visualization.h | 1 - tools/plot_lighthouse/main.c | 0 11 files changed, 255 insertions(+), 273 deletions(-) delete mode 100644 tools/lighthousefind_tori/find_tori_math.c delete mode 100644 tools/lighthousefind_tori/find_tori_math.h mode change 100755 => 100644 tools/plot_lighthouse/main.c diff --git a/redist/linmath.c b/redist/linmath.c index 60fbc21..ea44432 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -2,6 +2,7 @@ #include "linmath.h" #include +#include void cross3d( FLT * out, const FLT * a, const FLT * b ) { @@ -33,7 +34,7 @@ void scale3d( FLT * out, const FLT * a, FLT scalar ) void normalize3d( FLT * out, const FLT * in ) { - FLT r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] ); + FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]); out[0] = in[0] * r; out[1] = in[1] * r; out[2] = in[2] * r; @@ -65,7 +66,7 @@ void copy3d( FLT * out, const FLT * in ) FLT magnitude3d( FLT * a ) { - return sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] ); + return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); } FLT anglebetween3d( FLT * a, FLT * b ) @@ -77,7 +78,7 @@ FLT anglebetween3d( FLT * a, FLT * b ) FLT dot = dot3d( a, b ); if( dot < -0.9999999 ) return LINMATHPI; if( dot > 0.9999999 ) return 0; - return acos( dot ); + return FLT_ACOS(dot); } /////////////////////////////////////QUATERNIONS////////////////////////////////////////// @@ -106,12 +107,12 @@ void quatfromeuler( FLT * q, const FLT * euler ) FLT Y = euler[1]/2.0f; //pitch FLT Z = euler[2]/2.0f; //yaw - FLT cx = cosf(X); - FLT sx = sinf(X); - FLT cy = cosf(Y); - FLT sy = sinf(Y); - FLT cz = cosf(Z); - FLT sz = sinf(Z); + FLT cx = FLT_COS(X); + FLT sx = FLT_SIN(X); + FLT cy = FLT_COS(Y); + FLT sy = FLT_SIN(Y); + FLT cz = FLT_COS(Z); + FLT sz = FLT_SIN(Z); //Correct according to //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles @@ -125,9 +126,9 @@ void quatfromeuler( FLT * q, const FLT * euler ) void quattoeuler( FLT * euler, const FLT * q ) { //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) - euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) ); - euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) ); - euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) ); + euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); + euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); + euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3])); } void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) @@ -135,8 +136,8 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT v[3]; normalize3d( v, axis ); - FLT sn = sin(radians/2.0f); - q[0] = cos(radians/2.0f); + FLT sn = FLT_SIN(radians / 2.0f); + q[0] = FLT_COS(radians / 2.0f); q[1] = sn * v[0]; q[2] = sn * v[1]; q[3] = sn * v[2]; @@ -146,12 +147,12 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT quatmagnitude( const FLT * q ) { - return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); + return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); } FLT quatinvsqmagnitude( const FLT * q ) { - return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); + return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } @@ -296,13 +297,13 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) if ( 1 - (cosTheta*cosTheta) <= 0 ) sinTheta = 0; else - sinTheta = sqrt(1 - (cosTheta*cosTheta)); + sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta)); - FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions + FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions - if(fabs(Theta) < DEFAULT_EPSILON ) + if (FLT_FABS(Theta) < DEFAULT_EPSILON) quatcopy( q, qa ); - else if(fabs(sinTheta) < DEFAULT_EPSILON ) + else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) { quatadd( q, qa, qb ); quatscale( q, q, 0.5 ); @@ -311,10 +312,10 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) { FLT aside[4]; FLT bside[4]; - quatscale( bside, qb, sin( t * Theta ) ); - quatscale( aside, qa, sin((1-t)*Theta) ); + quatscale( bside, qb, FLT_SIN(t * Theta)); + quatscale( aside, qa, FLT_SIN((1 - t)*Theta)); quatadd( q, aside, bside ); - quatscale( q, q, 1./sinTheta ); + quatscale( q, q, ((FLT)1.)/sinTheta ); } } @@ -338,4 +339,176 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) } +// Matrix Stuff + +Matrix3x3 inverseM33(const Matrix3x3 mat) +{ + Matrix3x3 newMat; + for (int a = 0; a < 3; a++) + { + for (int b = 0; b < 3; b++) + { + newMat.val[a][b] = mat.val[a][b]; + } + } + + for (int i = 0; i < 3; i++) + { + for (int j = i + 1; j < 3; j++) + { + FLT tmp = newMat.val[i][j]; + newMat.val[i][j] = newMat.val[j][i]; + newMat.val[j][i] = tmp; + } + } + + return newMat; +} + +/////////////////////////////////////Matrix Rotations//////////////////////////////////// +//Originally from Stack Overflow +//Under cc by-sa 3.0 +// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw +// Copyright 2014 by Campbell Barton +// Copyright 2017 by Michael Turvey + +/** +* Calculate a rotation matrix from 2 normalized vectors. +* +* v1 and v2 must be unit length. +*/ +void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +{ + FLT axis[3]; + /* avoid calculating the angle */ + FLT angle_sin; + FLT angle_cos; + + cross3d(axis, v1, v2); + + angle_sin = normalize_v3(axis); + angle_cos = dot3d(v1, v2); + + if (angle_sin > FLT_EPSILON) { + axis_calc: + axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); + } + else { + /* Degenerate (co-linear) vectors */ + if (angle_cos > 0.0f) { + /* Same vectors, zero rotation... */ + unit_m3(m); + } + else { + /* Colinear but opposed vectors, 180 rotation... */ + get_orthogonal_vector(axis, v1); + normalize_v3(axis); + angle_sin = 0.0f; /* sin(M_PI) */ + angle_cos = -1.0f; /* cos(M_PI) */ + goto axis_calc; + } + } +} + +void get_orthogonal_vector(FLT out[3], const FLT in[3]) +{ +#ifdef USE_DOUBLE + const FLT x = fabs(in[0]); + const FLT y = fabs(in[1]); + const FLT z = fabs(in[2]); +#else + const FLT x = fabsf(in[0]); + const FLT y = fabsf(in[1]); + const FLT z = fabsf(in[2]); +#endif + + if (x > y && x > z) + { + // x is dominant + out[0] = -in[1] - in[2]; + out[1] = in[0]; + out[2] = in[0]; + } + else if (y > z) + { + // y is dominant + out[0] = in[1]; + out[1] = -in[0] - in[2]; + out[2] = in[1]; + } + else + { + // z is dominant + out[0] = in[2]; + out[1] = in[2]; + out[2] = -in[0] - in[1]; + } +} + +void unit_m3(FLT mat[3][3]) +{ + mat[0][0] = 1; + mat[0][1] = 0; + mat[0][2] = 0; + mat[1][0] = 0; + mat[1][1] = 1; + mat[1][2] = 0; + mat[2][0] = 0; + mat[2][1] = 0; + mat[2][2] = 1; +} + + +FLT normalize_v3(FLT vect[3]) +{ + FLT distance = dot3d(vect, vect); + + if (distance < 1.0e-35f) + { + // distance is too short, just go to zero. + vect[0] = 0; + vect[1] = 0; + vect[2] = 0; + distance = 0; + } + else + { + distance = FLT_SQRT((FLT)distance); + scale3d(vect, vect, 1.0f / distance); + } + + return distance; +} + +/* axis must be unit length */ +void axis_angle_normalized_to_mat3_ex( + FLT mat[3][3], const FLT axis[3], + const FLT angle_sin, const FLT angle_cos) +{ + FLT nsi[3], ico; + FLT n_00, n_01, n_11, n_02, n_12, n_22; + + ico = (1.0f - angle_cos); + nsi[0] = axis[0] * angle_sin; + nsi[1] = axis[1] * angle_sin; + nsi[2] = axis[2] * angle_sin; + + n_00 = (axis[0] * axis[0]) * ico; + n_01 = (axis[0] * axis[1]) * ico; + n_11 = (axis[1] * axis[1]) * ico; + n_02 = (axis[0] * axis[2]) * ico; + n_12 = (axis[1] * axis[2]) * ico; + n_22 = (axis[2] * axis[2]) * ico; + + mat[0][0] = n_00 + angle_cos; + mat[0][1] = n_01 + nsi[2]; + mat[0][2] = n_02 - nsi[1]; + mat[1][0] = n_01 - nsi[2]; + mat[1][1] = n_11 + angle_cos; + mat[1][2] = n_12 + nsi[0]; + mat[2][0] = n_02 + nsi[1]; + mat[2][1] = n_12 - nsi[0]; + mat[2][2] = n_22 + angle_cos; +} + diff --git a/redist/linmath.h b/redist/linmath.h index 5cc7b7d..530d291 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -10,14 +10,37 @@ #define PFTHREE(x) x[0], x[1], x[2] #define PFFOUR(x) x[0], x[1], x[2], x[3] -#define LINMATHPI 3.141592653589 +#define LINMATHPI ((FLT)3.141592653589) + +//uncomment the following line to use double precision instead of single precision. +//#define USE_DOUBLE + +#ifdef USE_DOUBLE + +#define FLT double +#define FLT_SQRT sqrt +#define FLT_SIN sin +#define FLT_COS cos +#define FLT_ACOS acos +#define FLT_ASIN asin +#define FLT_ATAN2 atan2 +#define FLT_FABS fabs + +#else -//If you want, you can define FLT to be double for double precision. -#ifndef FLT #define FLT float +#define FLT_SQRT sqrtf +#define FLT_SIN sinf +#define FLT_COS cosf +#define FLT_ACOS acosf +#define FLT_ASIN asinf +#define FLT_ATAN2 atan2f +#define FLT_FABS fabsf + #endif + //NOTE: Inputs may never be output with cross product. void cross3d( FLT * out, const FLT * a, const FLT * b ); @@ -64,6 +87,25 @@ void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ); void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ); void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ); +// Matrix Stuff + +typedef struct +{ + // row, column, (0,0) in upper left + FLT val[3][3]; +} Matrix3x3; + +Matrix3x3 inverseM33(const Matrix3x3 mat); +void get_orthogonal_vector(FLT out[3], const FLT in[3]); +void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]); +void unit_m3(FLT m[3][3]); +FLT normalize_v3(FLT n[3]); +void axis_angle_normalized_to_mat3_ex( + FLT mat[3][3], + const FLT axis[3], + const FLT angle_sin, + const FLT angle_cos); + #endif diff --git a/tools/lighthousefind/Makefile b/tools/lighthousefind/Makefile index 032ade7..cb073d9 100644 --- a/tools/lighthousefind/Makefile +++ b/tools/lighthousefind/Makefile @@ -1,6 +1,6 @@ all : lighthousefind -CFLAGS:=-g -O4 -DFLT=double -I../../redist -flto +CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto LDFLAGS:=$(CFLAGS) -lm lighthousefind : lighthousefind.o ../../redist/linmath.c diff --git a/tools/lighthousefind_tori/Makefile b/tools/lighthousefind_tori/Makefile index 3bdfdd6..b2dff64 100644 --- a/tools/lighthousefind_tori/Makefile +++ b/tools/lighthousefind_tori/Makefile @@ -1,7 +1,7 @@ -CFLAGS:=-g -O4 -DFLT=double -I../../redist -flto +CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto LDFLAGS:=$(CFLAGS) -lm all: - gcc -O3 -o lighthousefind-tori main.c find_tori_math.c torus_localizer.c visualization.c ../../redist/linmath.c $(LDFLAGS) + gcc -O3 -o lighthousefind-tori main.c torus_localizer.c visualization.c ../../redist/linmath.c $(LDFLAGS) clean: rm -f lighthousefind-tori diff --git a/tools/lighthousefind_tori/find_tori_math.c b/tools/lighthousefind_tori/find_tori_math.c deleted file mode 100644 index 9c305f3..0000000 --- a/tools/lighthousefind_tori/find_tori_math.c +++ /dev/null @@ -1,206 +0,0 @@ -#include -#include -#include "find_tori_math.h" - -// TODO: optimization potential to do in-place inverse for some places where this is used. -Matrix3x3 inverseM33(const Matrix3x3 mat) -{ - Matrix3x3 newMat; - for (int a = 0; a < 3; a++) - { - for (int b = 0; b < 3; b++) - { - newMat.val[a][b] = mat.val[a][b]; - } - } - - for (int i = 0; i < 3; i++) - { - for (int j = i + 1; j < 3; j++) - { - double tmp = newMat.val[i][j]; - newMat.val[i][j] = newMat.val[j][i]; - newMat.val[j][i] = tmp; - } - } - - return newMat; -} - - -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); -} - -//################################### -// The following code originally came from -// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw -// Need to check up on license terms and give proper attribution -// I think we'll be good with proper attribution, but don't want to assume without checking. - - - -/* -------------------------------------------------------------------- */ -/* Math Lib declarations */ - - - -/* -------------------------------------------------------------------- */ -/* Main function */ - -/** -* Calculate a rotation matrix from 2 normalized vectors. -* -* v1 and v2 must be unit length. -*/ -void rotation_between_vecs_to_mat3(double m[3][3], const double v1[3], const double v2[3]) -{ - double axis[3]; - /* avoid calculating the angle */ - double angle_sin; - double angle_cos; - - cross_v3_v3v3(axis, v1, v2); - - angle_sin = normalize_v3(axis); - angle_cos = dot_v3v3(v1, v2); - - if (angle_sin > FLT_EPSILON) { - axis_calc: - axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); - } - else { - /* Degenerate (co-linear) vectors */ - if (angle_cos > 0.0f) { - /* Same vectors, zero rotation... */ - unit_m3(m); - } - else { - /* Colinear but opposed vectors, 180 rotation... */ - ortho_v3_v3(axis, v1); - normalize_v3(axis); - angle_sin = 0.0f; /* sin(M_PI) */ - angle_cos = -1.0f; /* cos(M_PI) */ - goto axis_calc; - } - } -} - - -/* -------------------------------------------------------------------- */ -/* Math Lib */ - -void unit_m3(double m[3][3]) -{ - m[0][0] = m[1][1] = m[2][2] = 1.0; - m[0][1] = m[0][2] = 0.0; - m[1][0] = m[1][2] = 0.0; - m[2][0] = m[2][1] = 0.0; -} - -double dot_v3v3(const double a[3], const double b[3]) -{ - return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; -} - -void cross_v3_v3v3(double r[3], const double a[3], const double b[3]) -{ - r[0] = a[1] * b[2] - a[2] * b[1]; - r[1] = a[2] * b[0] - a[0] * b[2]; - r[2] = a[0] * b[1] - a[1] * b[0]; -} - -void mul_v3_v3fl(double r[3], const double a[3], double f) -{ - r[0] = a[0] * f; - r[1] = a[1] * f; - r[2] = a[2] * f; -} - -double normalize_v3_v3(double r[3], const double a[3]) -{ - double d = dot_v3v3(a, a); - - if (d > 1.0e-35f) { - d = sqrtf((float)d); - mul_v3_v3fl(r, a, 1.0f / d); - } - else { - d = r[0] = r[1] = r[2] = 0.0f; - } - - return d; -} - -double normalize_v3(double n[3]) -{ - return normalize_v3_v3(n, n); -} - -int axis_dominant_v3_single(const double vec[3]) -{ - const float x = fabsf((float)vec[0]); - const float y = fabsf((float)vec[1]); - const float z = fabsf((float)vec[2]); - return ((x > y) ? - ((x > z) ? 0 : 2) : - ((y > z) ? 1 : 2)); -} - -void ortho_v3_v3(double p[3], const double v[3]) -{ - const int axis = axis_dominant_v3_single(v); - - switch (axis) { - case 0: - p[0] = -v[1] - v[2]; - p[1] = v[0]; - p[2] = v[0]; - break; - case 1: - p[0] = v[1]; - p[1] = -v[0] - v[2]; - p[2] = v[1]; - break; - case 2: - p[0] = v[2]; - p[1] = v[2]; - p[2] = -v[0] - v[1]; - break; - } -} - -/* axis must be unit length */ -void axis_angle_normalized_to_mat3_ex( - double mat[3][3], const double axis[3], - const double angle_sin, const double angle_cos) -{ - double nsi[3], ico; - double n_00, n_01, n_11, n_02, n_12, n_22; - - ico = (1.0f - angle_cos); - nsi[0] = axis[0] * angle_sin; - nsi[1] = axis[1] * angle_sin; - nsi[2] = axis[2] * angle_sin; - - n_00 = (axis[0] * axis[0]) * ico; - n_01 = (axis[0] * axis[1]) * ico; - n_11 = (axis[1] * axis[1]) * ico; - n_02 = (axis[0] * axis[2]) * ico; - n_12 = (axis[1] * axis[2]) * ico; - n_22 = (axis[2] * axis[2]) * ico; - - mat[0][0] = n_00 + angle_cos; - mat[0][1] = n_01 + nsi[2]; - mat[0][2] = n_02 - nsi[1]; - mat[1][0] = n_01 - nsi[2]; - mat[1][1] = n_11 + angle_cos; - mat[1][2] = n_12 + nsi[0]; - mat[2][0] = n_02 + nsi[1]; - mat[2][1] = n_12 - nsi[0]; - mat[2][2] = n_22 + angle_cos; -} diff --git a/tools/lighthousefind_tori/find_tori_math.h b/tools/lighthousefind_tori/find_tori_math.h deleted file mode 100644 index a10c3fc..0000000 --- a/tools/lighthousefind_tori/find_tori_math.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef __FIND_TORI_MATH_H -#define __FIND_TORI_MATH_H - -#include "tori_includes.h" - -Matrix3x3 inverseM33(const Matrix3x3 mat); -double distance(Point a, Point b); - -void unit_m3(double m[3][3]); -double dot_v3v3(const double a[3], const double b[3]); -double normalize_v3(double n[3]); -void cross_v3_v3v3(double r[3], const double a[3], const double b[3]); -void mul_v3_v3fl(double r[3], const double a[3], double f); -void ortho_v3_v3(double p[3], const double v[3]); -void axis_angle_normalized_to_mat3_ex( - double mat[3][3], - const double axis[3], - const double angle_sin, - const double angle_cos); -void rotation_between_vecs_to_mat3(double m[3][3], const double v1[3], const double v2[3]); - - -#endif diff --git a/tools/lighthousefind_tori/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h index a6820b5..51cd04f 100644 --- a/tools/lighthousefind_tori/tori_includes.h +++ b/tools/lighthousefind_tori/tori_includes.h @@ -58,11 +58,6 @@ static const float DefaultPointsPerOuterDiameter = 60; -typedef struct -{ - // row, column, (0,0) in upper left - double val[3][3]; -} Matrix3x3; //#define TORI_DEBUG 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; diff --git a/tools/lighthousefind_tori/torus_localizer.h b/tools/lighthousefind_tori/torus_localizer.h index b8e7360..a42e37d 100644 --- a/tools/lighthousefind_tori/torus_localizer.h +++ b/tools/lighthousefind_tori/torus_localizer.h @@ -3,7 +3,6 @@ #include #include "tori_includes.h" -#include "find_tori_math.h" Point SolveForLighthouse(TrackedObject *obj, char doLogOutput); diff --git a/tools/lighthousefind_tori/visualization.h b/tools/lighthousefind_tori/visualization.h index e7f9475..f0263eb 100644 --- a/tools/lighthousefind_tori/visualization.h +++ b/tools/lighthousefind_tori/visualization.h @@ -4,7 +4,6 @@ #include #include "tori_includes.h" -#include "find_tori_math.h" extern int pointsWritten; diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c old mode 100755 new mode 100644 -- cgit v1.2.3