From 7ea248577178f45033802ba5cc2867f8a66d69f8 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 5 Feb 2017 23:01:34 -0700 Subject: Adding lighthousefind_tori --- tools/lighthousefind_tori/Makefile | 7 + tools/lighthousefind_tori/find_tori_math.c | 206 ++++++++++ tools/lighthousefind_tori/find_tori_math.h | 23 ++ tools/lighthousefind_tori/main.c | 219 +++++++++++ tools/lighthousefind_tori/tori_includes.h | 70 ++++ tools/lighthousefind_tori/torus_localizer.c | 561 ++++++++++++++++++++++++++++ tools/lighthousefind_tori/torus_localizer.h | 12 + tools/lighthousefind_tori/visualization.c | 94 +++++ tools/lighthousefind_tori/visualization.h | 48 +++ 9 files changed, 1240 insertions(+) create mode 100644 tools/lighthousefind_tori/Makefile create mode 100644 tools/lighthousefind_tori/find_tori_math.c create mode 100644 tools/lighthousefind_tori/find_tori_math.h create mode 100644 tools/lighthousefind_tori/main.c create mode 100644 tools/lighthousefind_tori/tori_includes.h create mode 100644 tools/lighthousefind_tori/torus_localizer.c create mode 100644 tools/lighthousefind_tori/torus_localizer.h create mode 100644 tools/lighthousefind_tori/visualization.c create mode 100644 tools/lighthousefind_tori/visualization.h diff --git a/tools/lighthousefind_tori/Makefile b/tools/lighthousefind_tori/Makefile new file mode 100644 index 0000000..3bdfdd6 --- /dev/null +++ b/tools/lighthousefind_tori/Makefile @@ -0,0 +1,7 @@ +CFLAGS:=-g -O4 -DFLT=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) +clean: + rm -f lighthousefind-tori diff --git a/tools/lighthousefind_tori/find_tori_math.c b/tools/lighthousefind_tori/find_tori_math.c new file mode 100644 index 0000000..9c305f3 --- /dev/null +++ b/tools/lighthousefind_tori/find_tori_math.c @@ -0,0 +1,206 @@ +#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 new file mode 100644 index 0000000..a10c3fc --- /dev/null +++ b/tools/lighthousefind_tori/find_tori_math.h @@ -0,0 +1,23 @@ +#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/main.c b/tools/lighthousefind_tori/main.c new file mode 100644 index 0000000..2fd517a --- /dev/null +++ b/tools/lighthousefind_tori/main.c @@ -0,0 +1,219 @@ +#include +#include +#include +#include +#include +#include +#include "linmath.h" +#include "torus_localizer.h" + +#define PTS 32 +#define MAX_CHECKS 40000 +#define MIN_HITS_FOR_VALID 10 + +FLT hmd_points[PTS * 3]; +FLT hmd_norms[PTS * 3]; + +// index for a given sensor is (2*sensor + is_sensor_y ? 1 : 0) +FLT hmd_point_angles[PTS * 2]; +int hmd_point_counts[PTS * 2]; +int best_hmd_target = 0; + +static void printTrackedObject(TrackedObject *to) +{ + for (unsigned int i = 0; i < to->numSensors; i++) + { + printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n", + i, + to->sensor[i].theta, + to->sensor[i].phi, + to->sensor[i].point.x, + to->sensor[i].point.y, + to->sensor[i].point.z + ); + } +} + +static void runTheNumbers() +{ + TrackedObject *to; + + to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor))); + + int sensorCount = 0; + + for (int i = 0; i < PTS; i++) + { + // if there are enough valid counts for both the x and y sweeps for sensor i + if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) && + (hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID)) + { + to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0]; + to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1]; + to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2]; + to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0]; + to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1]; + sensorCount++; + } + } + + to->numSensors = sensorCount; + + printf("Using %d sensors to find lighthouse.\n", sensorCount); + + Point lh = SolveForLighthouse(to, 0); + + printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); + + //printTrackedObject(to); + free(to); +} + + +int LoadData(char Camera, const char * datafile) +{ + + + //First, read the positions of all the sensors on the HMD. + FILE * f = fopen("HMD_points.csv", "r"); + int pt = 0; + if (!f) + { + fprintf(stderr, "error: can't open hmd points.\n"); + return -5; + } + while (!feof(f) && !ferror(f) && pt < PTS) + { + float fx, fy, fz; + int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz); + hmd_points[pt * 3 + 0] = fx; + hmd_points[pt * 3 + 1] = fy; + hmd_points[pt * 3 + 2] = fz; + pt++; + if (r != 3) + { + fprintf(stderr, "Not enough entries on line %d of points\n", pt); + return -8; + } + } + if (pt < PTS) + { + fprintf(stderr, "Not enough points.\n"); + return -9; + } + fclose(f); + printf("Loaded %d points\n", pt); + + //Read all the normals on the HMD into hmd_norms. + f = fopen("HMD_normals.csv", "r"); + int nrm = 0; + if (!f) + { + fprintf(stderr, "error: can't open hmd points.\n"); + return -5; + } + while (!feof(f) && !ferror(f) && nrm < PTS) + { + float fa, fb, fc; + int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc); + hmd_norms[nrm * 3 + 0] = fa; + hmd_norms[nrm * 3 + 1] = fb; + hmd_norms[nrm * 3 + 2] = fc; + nrm++; + if (r != 3) + { + fprintf(stderr, "Not enough entries on line %d of normals\n", nrm); + return -8; + } + } + if (nrm < PTS) + { + fprintf(stderr, "Not enough points.\n"); + return -9; + } + if (nrm != pt) + { + fprintf(stderr, "point/normal counts disagree.\n"); + return -9; + } + fclose(f); + printf("Loaded %d norms\n", nrm); + + //Actually load the processed data! + int xck = 0; + f = fopen(datafile, "r"); + if (!f) + { + fprintf(stderr, "Error: cannot open %s\n", datafile); + exit(-11); + } + int lineno = 0; + while (!feof(f)) + { + //Format: + // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936 + lineno++; + char devn[10]; + char inn[10]; + int id; + int pointct; + double avgTime; + double avgLen; + double stddevTime; + double stddevLen; + int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen); + if (ct == 0) continue; + if (ct != 8) + { + fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno); + } + if (strcmp(devn, "HMD") != 0) continue; + + if (inn[0] != Camera) continue; + + int isy = inn[1] == 'Y'; + +// hmd_point_angles[id * 2 + isy] = (avgTime - 200000) / 200000 * 3.1415926535 / 2.0; + hmd_point_angles[id * 2 + isy] = (FLT)(avgTime / 48000000 * 60 * M_PI * 2); + + hmd_point_counts[id * 2 + isy] = pointct; + } + fclose(f); + + + int targpd; + int maxhits = 0; + + for (targpd = 0; targpd < PTS; targpd++) + { + int hits = hmd_point_counts[targpd * 2 + 0]; + if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1]; + //Need an X and a Y lock. + + if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; } + } + if (maxhits < MIN_HITS_FOR_VALID) + { + fprintf(stderr, "Error: Not enough data for a primary fix.\n"); + } + + return 0; +} + + +int main(int argc, char ** argv) +{ + if (argc != 3) + { + fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n"); + exit(-1); + } + + //Load either 'L' (LH1) or 'R' (LH2) data. + if (LoadData(argv[1][0], argv[2])) return 5; + + + runTheNumbers(); + + return 0; +} diff --git a/tools/lighthousefind_tori/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h new file mode 100644 index 0000000..a6820b5 --- /dev/null +++ b/tools/lighthousefind_tori/tori_includes.h @@ -0,0 +1,70 @@ +#ifndef __TORI_INCLUDES_H +#define __TORI_INCLUDES_H + +#include +#include +#include + + +typedef struct +{ + double x; + double y; + double z; +} Point; + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846264338327 +#endif + +#define SQUARED(x) ((x)*(x)) + +typedef union +{ + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; +// float float_value; + uint32_t long_value; +} RGBValue; + +static RGBValue RED = { .Red = 255, .Green = 0, .Blue = 0, .Alpha = 125 }; +static RGBValue GREEN = { .Red = 0, .Green = 255, .Blue = 0, .Alpha = 125 }; +static RGBValue BLUE = { .Red = 0, .Green = 0, .Blue = 255, .Alpha = 125 }; + +static const double WORLD_BOUNDS = 100; +#define MAX_TRACKED_POINTS 40 + +static const float DefaultPointsPerOuterDiameter = 60; + + + +typedef struct +{ + // row, column, (0,0) in upper left + double val[3][3]; +} Matrix3x3; + + +//#define TORI_DEBUG + +#endif diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c new file mode 100644 index 0000000..58e4938 --- /dev/null +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -0,0 +1,561 @@ +#include +#include +#include +#include +#include "tori_includes.h" +#include "find_tori_math.h" +#include "visualization.h" + + +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 }; + + normalize_v3(v2); + + rotation_between_vecs_to_mat3(result.val, v1, v2); + + return result; +} + +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; + + return q; +} + +double angleFromPoints(Point p1, Point p2, Point center) +{ + Point v1, v2, v1norm, v2norm; + v1.x = p1.x - center.x; + v1.y = p1.y - center.y; + v1.z = p1.z - center.z; + + v2.x = p2.x - center.x; + v2.y = p2.y - center.y; + v2.z = p2.z - center.z; + + double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); + v1norm.x = v1.x / v1mag; + v1norm.y = v1.y / v1mag; + v1norm.z = v1.z / v1mag; + + double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); + v2norm.x = v2.x / v2mag; + v2norm.y = v2.y / v2mag; + v2norm.z = v2.z / v2mag; + + double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; + + double angle = acos(res); + + return angle; +} + +Point midpoint(Point a, Point b) +{ + Point m; + m.x = (a.x + b.x) / 2; + m.y = (a.y + b.y) / 2; + m.z = (a.z + b.z) / 2; + + return m; +} + + + + +// This is the second incarnation of the torus generator. It is intended to differ from the initial torus generator by +// producing a point cloud of a torus where the points density is more uniform across the torus. This will allow +// us to be more efficient in finding a solution. +void partialTorusGenerator( + Point p1, + Point p2, + double toroidalStartAngle, + double toroidalEndAngle, + double poloidalStartAngle, + double poloidalEndAngle, + double lighthouseAngle, + double toroidalPrecision, + Point **pointCloud) +{ + double poloidalRadius = 0; + double toroidalRadius = 0; + + Point m = midpoint(p1, p2); + double distanceBetweenPoints = distance(p1, p2); + + // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally + // thinking the tori converge at the location of the tracked object instead of at the lighthouse. + double centralAngleToIgnore = lighthouseAngle * 3; + + Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); + + toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); + + poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); + + double poloidalPrecision = M_PI * 2 / toroidalPrecision; + + //unsigned int pointCount = toroidalPrecision * toroidalPrecision / 2 * (toroidalEndAngle - toroidalStartAngle) / (M_PI * 2) * (poloidalEndAngle - poloidalStartAngle) / (M_PI * 1); + //unsigned int pointCount = (unsigned int)(toroidalPrecision * ((M_PI - lighthouseAngle) * 2 / poloidalPrecision + 1) + 1); + // TODO: This calculation of the number of points that we will generate is excessively large (probably by about a factor of 2 or more) We can do better. + //float pointEstimate = (pointCount + 1000) * sizeof(Point) * 2 * M_PI / (toroidalEndAngle - toroidalStartAngle); + + unsigned int pointCount = 0; + + for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) + { + // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle + // We do this so our point cloud will have a more even distribution of points across the surface of the torus. + double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; + + double step_distance = 2 * M_PI / steps; + + pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2); + } + + *pointCloud = malloc(pointCount * sizeof(Point) ); + + assert(0 != *pointCloud); + + (*pointCloud)[pointCount - 1].x = -1000; + (*pointCloud)[pointCount - 1].y = -1000; + (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now. + + size_t currentPoint = 0; + + for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) + { + // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle + // We do this so our point cloud will have a more even distribution of points across the surface of the torus. + double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; + + double step_distance = 2 * M_PI / steps; + + //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) + for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance) + { + if (currentPoint >= pointCount - 1) + { + int a = 0; + } + assert(currentPoint < pointCount - 1); + (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); + (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); + (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep); + (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m); + + // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up + // and hence we know that nothing with a negative z value is a possible lightouse location. + // Before this code can go live, we'll have to take the normals into account and remove this hack. + if ((*pointCloud)[currentPoint].z > 0) + { + currentPoint++; + } + } + } + +#ifdef TORI_DEBUG + printf("%d / %d\n", currentPoint, pointCount); +#endif + (*pointCloud)[currentPoint].x = -1000; + (*pointCloud)[currentPoint].y = -1000; + (*pointCloud)[currentPoint].z = -1000; +} + +void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointCloud) +{ + + double centralAngleToIgnore = lighthouseAngle * 6; + + centralAngleToIgnore = 20.0 / 180.0 * M_PI; + + partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud); + + return; +} + + +// What we're doing here is: +// * Given a point in space +// * And points and a lighthouse angle that implicitly define a torus +// * for that torus, what is the toroidal angle of the plane that will go through that point in space +// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space? +// +// Given the toroidal and poloidal angles of a "good estimate" of a solution position, a caller of this function +// will be able to "draw" the point cloud of a torus in just the surface of the torus near the point in space. +// 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, + Point point, + double *toroidalAngle, + double *poloidalAngle) +{ + // 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); + + // 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. + // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. + rot = inverseM33(rot); + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(torusP1, torusP2); + + // 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 + Point pointF; + + // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also + // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system + // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. + pointF.x = point.x - m.x; + pointF.y = point.y - m.y; + pointF.z = point.z - m.z; + + // now we'll undo the rotation part. + pointF = RotateAndTranslatePoint(pointF, rot, origin); + + // hooray, now pointF is in our more-friendly coordinate system. + + // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. + // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the + // angle between a vector to pointF and a vector along the x axis. + + *toroidalAngle = atan(pointF.y / pointF.x); + if (pointF.x < 0) + { + *toroidalAngle += M_PI; + } + + // SCORE!! We've got the toroidal angle. We're half done! + + // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to + // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle + // so that the point we're focusin on will lie on the x/z plane. We then should translate the + // torus so that the center of the poloidal circle is at the origin. At that point, it will + // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a + // vector from the origin to the point. + + // okay, instead of rotating the torus & point by the toroidal angle to get the point on + // the xz plane, we're going to take advantage of the radial symmetry of the torus + // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't + // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point + // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent + // this new coordinate system + + Point pointG; + pointG.z = pointF.z; + pointG.y = 0; + pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); + + // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, + // our next step will be to shift it down so that the center of the poloidal circle is at the origin. + // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat + // 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 poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); + + // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. + // The shift along the X axis will be the toroidal radius. + + Point pointH; + pointH.z = pointG.z; + pointH.y = pointG.y; + pointH.x = pointG.x - toroidalRadius; + + // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, + // that will be the poloidal angle we're looking for. (crosses fingers) + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + // Wow, that ended up being not so much code, but a lot of interesting trig. + // can't remember the last time I spent so much time working through each line of code. + + return; +} + +double FindSmallestDistance(Point p, Point* cloud) +{ + Point *cp = cloud; + double smallestDistance = 10000000000000.0; + + while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) + { + double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z)); + if (distance < smallestDistance) + { + smallestDistance = distance; + } + cp++; + } + smallestDistance = sqrt(smallestDistance); + return smallestDistance; +} + +// Given a cloud and a list of clouds, find the point on masterCloud that best matches clouds. +Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds) +{ + + Point bestMatch = { 0 }; + double bestDistance = 10000000000000.0; + Point *cp = masterCloud; + int point = 0; + while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) + { + point++; +#ifdef TORI_DEBUG + if (point % 100 == 0) + { + printf("."); + } +#endif + double currentDistance = 0; + for (int i = 0; i < numClouds; i++) + { + if (clouds[i] == masterCloud) + { + continue; + } + Point* cloud = clouds[i]; + currentDistance += FindSmallestDistance(*cp, cloud); + } + + if (currentDistance < bestDistance) + { + bestDistance = currentDistance; + bestMatch = *cp; + } + cp++; + } + + return bestMatch; +} + + +#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)); + double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta)); + + return angle; +} +double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) +{ + double p = (a->phi - b->phi); + double d = (a->theta - b->theta); + + double adjd = sin((a->phi + b->phi) / 2); + double adjP = sin((a->theta + b->theta) / 2); + double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; +} +Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +{ + PointsAndAngle pna[MAX_POINT_PAIRS]; + //Point lh = { 10, 0, 200 }; + + size_t pnaCount = 0; + for (unsigned int i = 0; i < obj->numSensors; i++) + { + for (unsigned int j = 0; j < i; j++) + { + if (pnaCount < MAX_POINT_PAIRS) + { + pna[pnaCount].a = obj->sensor[i].point; + pna[pnaCount].b = obj->sensor[j].point; + + pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + //double tmp = angleFromPoints(pna[pnaCount].a, pna[pnaCount].b, lh); + + pnaCount++; + } + } + } + + //Point **pointCloud = malloc(sizeof(Point*)* pnaCount); + Point **pointCloud = malloc(sizeof(void*)* pnaCount); + + FILE *f = NULL; + if (doLogOutput) + { + f = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(f); + writeAxes(f); + } + + for (unsigned int i = 0; i < pnaCount; i++) + { + torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i])); + if (doLogOutput) + { + writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]); + } + + } + + Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount); + + if (doLogOutput) + { + markPointWithStar(f, bestMatchA, 0xFF0000); + } +#ifdef TORI_DEBUG + printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z); +#endif + // Now, let's add an extra patch or torus near the point we just found. + + double toroidalAngle = 0; + double poloidalAngle = 0; + + + + Point **pointCloud2 = malloc(sizeof(void*)* pnaCount); + + for (unsigned int i = 0; i < pnaCount; i++) + { + estimateToroidalAndPoloidalAngleOfPoint( + pna[i].a, + pna[i].b, + pna[i].angle, + bestMatchA, + &toroidalAngle, + &poloidalAngle); + + partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.2, toroidalAngle + 0.2, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i])); + + if (doLogOutput) + { + writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]); + } + + } + + Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount); + if (doLogOutput) + { + markPointWithStar(f, bestMatchB, 0x00FF00); + } +#ifdef TORI_DEBUG + printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z); +#endif + + Point **pointCloud3 = malloc(sizeof(void*)* pnaCount); + + for (unsigned int i = 0; i < pnaCount; i++) + { + estimateToroidalAndPoloidalAngleOfPoint( + pna[i].a, + pna[i].b, + pna[i].angle, + bestMatchB, + &toroidalAngle, + &poloidalAngle); + + partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.05, poloidalAngle + 0.05, pna[i].angle, 3000, &(pointCloud3[i])); + + if (doLogOutput) + { + writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]); + } + + } + + Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount); + if (doLogOutput) + { + markPointWithStar(f, bestMatchC, 0xFFFFFF); + } +#ifdef TORI_DEBUG + printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z); +#endif + + + if (doLogOutput) + { + updateHeader(f); + fclose(f); + } + + return bestMatchC; +} + +static Point makeUnitPoint(Point *p) +{ + Point newP; + double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z); + newP.x = p->x / r; + newP.y = p->y / r; + newP.z = p->z / r; + + return newP; +} + +static double getPhi(Point p) +{ + // double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z))); + // double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z); + double phi = atan(p.x / p.z); + return phi; +} + +static double getTheta(Point p) +{ + //double theta = atan(p.y / p.x); + double theta = atan(p.x / p.y); + return theta; +} + +// subtraction +static Point PointSub(Point a, Point b) +{ + Point newPoint; + + newPoint.x = a.x - b.x; + newPoint.y = a.y - b.y; + newPoint.z = a.z - b.z; + + return newPoint; +} + + diff --git a/tools/lighthousefind_tori/torus_localizer.h b/tools/lighthousefind_tori/torus_localizer.h new file mode 100644 index 0000000..b8e7360 --- /dev/null +++ b/tools/lighthousefind_tori/torus_localizer.h @@ -0,0 +1,12 @@ +#ifndef __TORUS_LOCALIZER_H +#define __TORUS_LOCALIZER_H +#include + +#include "tori_includes.h" +#include "find_tori_math.h" + +Point SolveForLighthouse(TrackedObject *obj, char doLogOutput); + + + +#endif diff --git a/tools/lighthousefind_tori/visualization.c b/tools/lighthousefind_tori/visualization.c new file mode 100644 index 0000000..d348c2d --- /dev/null +++ b/tools/lighthousefind_tori/visualization.c @@ -0,0 +1,94 @@ +#include "visualization.h" +#include "tori_includes.h" + +int pointsWritten = 0; + + +void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) +{ + fprintf(file, "%f %f %f %u\n", x, y, z, rgb); + pointsWritten++; +} + +void updateHeader(FILE * file) +{ + fseek(file, 0x4C, SEEK_SET); + fprintf(file, "%d", pointsWritten); + fseek(file, 0x7C, SEEK_SET); + fprintf(file, "%d", pointsWritten); +} +void writeAxes(FILE * file) +{ + double scale = 5; + for (double i = 0; i < scale; i = i + scale / 1000) + { + writePoint(file, i, 0, 0, 255); + } + for (double i = 0; i < scale; i = i + scale / 1000) + { + if ((int)(i / (scale / 5)) % 2 == 1) + { + writePoint(file, 0, i, 0, 255 << 8); + } + } + for (double i = 0; i < scale; i = i + scale / 10001) + { + if ((int)(i / (scale / 10)) % 2 == 1) + { + writePoint(file, 0, 0, i, 255 << 16); + } + } +} + +void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) +{ + int max = 50; + for (int i = 0; i < max; i++) + { + writePoint(file, + (a.x*i + b.x*(max - i)) / max, + (a.y*i + b.y*(max - i)) / max, + (a.z*i + b.z*(max - i)) / max, + color); + } +} + +void writePcdHeader(FILE * file) +{ + fprintf(file, "VERSION 0.7\n"); + fprintf(file, "FIELDS x y z rgb\n"); + fprintf(file, "SIZE 4 4 4 4\n"); + fprintf(file, "TYPE F F F U\n"); + fprintf(file, "COUNT 1 1 1 1\n"); + fprintf(file, "WIDTH \n"); + fprintf(file, "HEIGHT 1\n"); + fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n"); + fprintf(file, "POINTS \n"); + fprintf(file, "DATA ascii\n"); + + //fprintf(file, "100000.0, 100000.0, 100000\n"); + +} + +void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) +{ + Point *currentPoint = pointCloud; + + while (currentPoint->x != -1000 || currentPoint->y != -1000 || currentPoint->z != -1000) + { + writePoint(f, currentPoint->x, currentPoint->y, currentPoint->z, Color); + currentPoint++; + } +} + +void markPointWithStar(FILE *file, Point point, unsigned int color) +{ + double i; + for (i = -0.8; i <= 0.8; i = i + 0.0025) + { + writePoint(file, point.x + i, point.y, point.z, color); + writePoint(file, point.x, point.y + i, point.z, color); + writePoint(file, point.x, point.y, point.z + i, color); + } + +} diff --git a/tools/lighthousefind_tori/visualization.h b/tools/lighthousefind_tori/visualization.h new file mode 100644 index 0000000..e7f9475 --- /dev/null +++ b/tools/lighthousefind_tori/visualization.h @@ -0,0 +1,48 @@ + +#ifndef __VISUALIZATION_H +#define __VISUALIZATION_H + +#include +#include "tori_includes.h" +#include "find_tori_math.h" + +extern int pointsWritten; + +void writePoint(FILE *file, double x, double y, double z, unsigned int rgb); + +void updateHeader(FILE * file); + +void writeAxes(FILE * file); + +void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color); + +void writePcdHeader(FILE * file); + +void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color); + +void markPointWithStar(FILE *file, Point point, unsigned int color); + +#define MAX_COLORS 18 +static unsigned int COLORS[] = { + 0x00FFFF, + 0xFF00FF, + 0xFFFF00, + 0xFF0000, + 0x00FF00, + 0x0000FF, + 0x0080FF, + 0x8000FF, + 0x80FF00, + 0x00FF80, + 0xFF0080, + 0xFF8000, + 0x008080, + 0x800080, + 0x808000, + 0x000080, + 0x008000, + 0x800000 +}; +#endif + + -- cgit v1.2.3 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 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) --- redist/linmath.c | 185 ++++++++++++++++++++++------ redist/linmath.h | 4 +- tools/lighthousefind_tori/main.c | 2 +- tools/lighthousefind_tori/torus_localizer.c | 14 +++ 4 files changed, 165 insertions(+), 40 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index ea44432..f5f6b22 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -64,7 +64,7 @@ void copy3d( FLT * out, const FLT * in ) out[2] = in[2]; } -FLT magnitude3d( FLT * a ) +FLT magnitude3d(const FLT * a ) { return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); } @@ -88,12 +88,17 @@ FLT anglebetween3d( FLT * a, FLT * b ) -void quatsetnone( FLT * q ) +void quatsetnone(FLT * q) { - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatcopy( FLT * qout, const FLT * qin ) +void quatsetidentity(FLT * q) +{ + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; +} + +void quatcopy(FLT * qout, const FLT * qin) { qout[0] = qin[0]; qout[1] = qin[1]; @@ -162,47 +167,79 @@ void quatnormalize( FLT * qout, const FLT * qin ) quatscale( qout, qin, imag ); } -void quattomatrix( FLT * matrix44, const FLT * qin ) +void quattomatrix(FLT * matrix44, const FLT * qin) { - FLT q[4]; - quatnormalize( q, qin ); - - //Reduced calulation for speed - FLT xx = 2*q[0]*q[0]; - FLT xy = 2*q[0]*q[1]; - FLT xz = 2*q[0]*q[2]; - FLT xw = 2*q[0]*q[3]; - - FLT yy = 2*q[1]*q[1]; - FLT yz = 2*q[1]*q[2]; - FLT yw = 2*q[1]*q[3]; - - FLT zz = 2*q[2]*q[2]; - FLT zw = 2*q[2]*q[3]; + FLT q[4]; + quatnormalize(q, qin); - //opengl major - matrix44[0] = 1-yy-zz; - matrix44[1] = xy-zw; - matrix44[2] = xz+yw; - matrix44[3] = 0; + //Reduced calulation for speed + FLT xx = 2 * q[0] * q[0]; + FLT xy = 2 * q[0] * q[1]; + FLT xz = 2 * q[0] * q[2]; + FLT xw = 2 * q[0] * q[3]; - matrix44[4] = xy+zw; - matrix44[5] = 1-xx-zz; - matrix44[6] = yz-xw; - matrix44[7] = 0; + FLT yy = 2 * q[1] * q[1]; + FLT yz = 2 * q[1] * q[2]; + FLT yw = 2 * q[1] * q[3]; - matrix44[8] = xz-yw; - matrix44[9] = yz+xw; - matrix44[10] = 1-xx-yy; - matrix44[11] = 0; + FLT zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; - matrix44[12] = 0; - matrix44[13] = 0; - matrix44[14] = 0; - matrix44[15] = 1; + //opengl major + matrix44[0] = 1 - yy - zz; + matrix44[1] = xy - zw; + matrix44[2] = xz + yw; + matrix44[3] = 0; + + matrix44[4] = xy + zw; + matrix44[5] = 1 - xx - zz; + matrix44[6] = yz - xw; + matrix44[7] = 0; + + matrix44[8] = xz - yw; + matrix44[9] = yz + xw; + matrix44[10] = 1 - xx - yy; + matrix44[11] = 0; + + matrix44[12] = 0; + matrix44[13] = 0; + matrix44[14] = 0; + matrix44[15] = 1; } -void quatgetconjugate( FLT * qout, const FLT * qin ) +void quattomatrix33(FLT * matrix33, const FLT * qin) +{ + FLT q[4]; + quatnormalize(q, qin); + + //Reduced calulation for speed + FLT xx = 2 * q[0] * q[0]; + FLT xy = 2 * q[0] * q[1]; + FLT xz = 2 * q[0] * q[2]; + FLT xw = 2 * q[0] * q[3]; + + FLT yy = 2 * q[1] * q[1]; + FLT yz = 2 * q[1] * q[2]; + FLT yw = 2 * q[1] * q[3]; + + FLT zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; + + //opengl major + matrix33[0] = 1 - yy - zz; + matrix33[1] = xy - zw; + matrix33[2] = xz + yw; + + matrix33[3] = xy + zw; + matrix33[4] = 1 - xx - zz; + matrix33[5] = yz - xw; + + matrix33[6] = xz - yw; + matrix33[7] = yz + xw; + matrix33[8] = 1 - xx - yy; +} + +void quatgetconjugate(FLT * qout, const FLT * qin) { qout[0] = qin[0]; qout[1] = -qin[1]; @@ -365,6 +402,78 @@ Matrix3x3 inverseM33(const Matrix3x3 mat) return newMat; } +void rotation_between_vecs_to_m3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +{ + FLT q[4]; + + getRotationTo(q, v1, v2); + + quattomatrix33(&(m[0][0]), q); +} + +// This function based on code from Object-oriented Graphics Rendering Engine +// Copyright(c) 2000 - 2012 Torus Knot Software Ltd +// under MIT license +// http://www.ogre3d.org/docs/api/1.9/_ogre_vector3_8h_source.html + +/** Gets the shortest arc quaternion to rotate this vector to the destination +vector. +@remarks +If you call this with a dest vector that is close to the inverse +of this vector, we will rotate 180 degrees around a generated axis if +since in this case ANY axis of rotation is valid. +*/ +void getRotationTo(FLT *q, const FLT *src, const FLT *dest) +{ + // Based on Stan Melax's article in Game Programming Gems + + // Copy, since cannot modify local + FLT v0[3]; + FLT v1[3]; + normalize3d(v0, src); + normalize3d(v1, dest); + + FLT d = dot3d(v0, v1);// v0.dotProduct(v1); + // If dot == 1, vectors are the same + if (d >= 1.0f) + { + quatsetidentity(q); + return; + } + if (d < (1e-6f - 1.0f)) + { + // Generate an axis + FLT unitX[3] = { 1, 0, 0 }; + FLT unitY[3] = { 0, 1, 0 }; + + FLT axis[3]; + cross3d(axis, unitX, src); // pick an angle + if ((axis[0] < 1.0e-35f) && + (axis[1] < 1.0e-35f) && + (axis[2] < 1.0e-35f)) // pick another if colinear + { + cross3d(axis, unitY, src); + } + normalize3d(axis, axis); + quatfromaxisangle(q, axis, LINMATHPI); + } + else + { + FLT s = FLT_SQRT((1 + d) * 2); + FLT invs = 1 / s; + + FLT c[3]; + cross3d(c, v0, v1); + + q[0] = c[0] * invs; + q[1] = c[1] * invs; + q[2] = c[2] * invs; + q[3] = s * 0.5f; + + quatnormalize(q, q); + } +} + /////////////////////////////////////Matrix Rotations//////////////////////////////////// //Originally from Stack Overflow //Under cc by-sa 3.0 diff --git a/redist/linmath.h b/redist/linmath.h index 530d291..3b11c1b 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -59,7 +59,7 @@ int compare3d( const FLT * a, const FLT * b, FLT epsilon ); void copy3d( FLT * out, const FLT * in ); -FLT magnitude3d( FLT * a ); +FLT magnitude3d(const FLT * a ); FLT anglebetween3d( FLT * a, FLT * b ); @@ -87,6 +87,8 @@ 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 ); +void getRotationTo(FLT *q, const FLT *src, const FLT *dest); + // Matrix Stuff typedef struct 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 From 033881d5848726a783bf365f64a814ac0c511e2b Mon Sep 17 00:00:00 2001 From: Volkan Sagcan Date: Wed, 8 Feb 2017 15:00:01 +0100 Subject: Update URL collection of YouTube livestreams --- README.md | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index bb7e520..147a903 100644 --- a/README.md +++ b/README.md @@ -4,13 +4,16 @@ Discord: https://discordapp.com/invite/7QbCAGS -First livestream: https://www.youtube.com/watch?v=sv_AVI9kHN4 - -Second livestream: https://www.youtube.com/watch?v=gFyEbGQ88s4 - -First summary livestream: https://www.youtube.com/watch?v=oHJkpNakswM - -Third livestream: https://www.youtube.com/watch?v=RExji5EtSzE +## Livestream collection +| Note | Youtube URL | +| -------------------------------------- | ------------------------------------------- | +| First livestream | https://www.youtube.com/watch?v=sv_AVI9kHN4 | +| Second livestream | https://www.youtube.com/watch?v=gFyEbGQ88s4 | +| Summary of first and second livestream | https://www.youtube.com/watch?v=oHJkpNakswM | +| Third livestream | https://www.youtube.com/watch?v=RExji5EtSzE | +| Fourth livestream | https://www.youtube.com/watch?v=fces1O7kWGY | +| Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | +| Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e -- cgit v1.2.3 From 29400a2063fc740c3c33d35649d6f6a4504b6745 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Wed, 8 Feb 2017 11:47:20 -0500 Subject: Update README.md --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 147a903..f90590f 100644 --- a/README.md +++ b/README.md @@ -5,15 +5,15 @@ Discord: https://discordapp.com/invite/7QbCAGS ## Livestream collection -| Note | Youtube URL | -| -------------------------------------- | ------------------------------------------- | -| First livestream | https://www.youtube.com/watch?v=sv_AVI9kHN4 | -| Second livestream | https://www.youtube.com/watch?v=gFyEbGQ88s4 | -| Summary of first and second livestream | https://www.youtube.com/watch?v=oHJkpNakswM | -| Third livestream | https://www.youtube.com/watch?v=RExji5EtSzE | -| Fourth livestream | https://www.youtube.com/watch?v=fces1O7kWGY | -| Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | -| Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | +| Note | Youtube URL | Run time | +| -------------------------------------- | ------------------------------------------- | -------- | +| First livestream | https://www.youtube.com/watch?v=sv_AVI9kHN4 | 5:01:25 | +| Second livestream | https://www.youtube.com/watch?v=gFyEbGQ88s4 | 4:03:26 | +| Summary of first and second livestream | https://www.youtube.com/watch?v=oHJkpNakswM | 23:00 | +| Third livestream | https://www.youtube.com/watch?v=RExji5EtSzE | 4:11:16 | +| Fourth livestream | https://www.youtube.com/watch?v=fces1O7kWGY | 4:50:33 | +| Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | 3:13:38 | +| Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | 3:44:49 | Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e -- cgit v1.2.3 From ae522f8a06848d467c835d87772580fa7cceb5cd Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Feb 2017 11:42:46 -0700 Subject: Replaced rotation algorithm & cleanup --- redist/linmath.c | 594 ++++++++++---------- redist/linmath.h | 20 +- tools/lighthousefind_tori/main.c | 334 ++++++----- tools/lighthousefind_tori/tori_includes.h | 39 +- tools/lighthousefind_tori/torus_localizer.c | 830 ++++++++++++++-------------- tools/lighthousefind_tori/visualization.c | 116 ++-- tools/lighthousefind_tori/visualization.h | 36 +- 7 files changed, 992 insertions(+), 977 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index f5f6b22..d5d54e3 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -34,7 +34,7 @@ void scale3d( FLT * out, const FLT * a, FLT scalar ) void normalize3d( FLT * out, const FLT * in ) { - FLT r = ((FLT)1.) / FLT_SQRT(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; @@ -66,7 +66,7 @@ void copy3d( FLT * out, const FLT * in ) FLT magnitude3d(const FLT * a ) { - return FLT_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 ) @@ -78,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 FLT_ACOS(dot); + return FLT_ACOS(dot); } /////////////////////////////////////QUATERNIONS////////////////////////////////////////// @@ -90,12 +90,12 @@ FLT anglebetween3d( FLT * a, FLT * b ) void quatsetnone(FLT * q) { - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } void quatsetidentity(FLT * q) { - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; } void quatcopy(FLT * qout, const FLT * qin) @@ -112,12 +112,12 @@ void quatfromeuler( FLT * q, const FLT * euler ) FLT Y = euler[1]/2.0f; //pitch FLT Z = euler[2]/2.0f; //yaw - 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); + 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 @@ -131,9 +131,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] = 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])); + 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 ) @@ -141,8 +141,8 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT v[3]; normalize3d( v, axis ); - FLT sn = FLT_SIN(radians / 2.0f); - q[0] = FLT_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]; @@ -152,7 +152,7 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT quatmagnitude( const FLT * q ) { - return FLT_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 ) @@ -169,74 +169,74 @@ void quatnormalize( FLT * qout, const FLT * qin ) void quattomatrix(FLT * matrix44, const FLT * qin) { - FLT q[4]; - quatnormalize(q, qin); + FLT q[4]; + quatnormalize(q, qin); - //Reduced calulation for speed - FLT xx = 2 * q[0] * q[0]; - FLT xy = 2 * q[0] * q[1]; - FLT xz = 2 * q[0] * q[2]; - FLT xw = 2 * q[0] * q[3]; + //Reduced calulation for speed + FLT xx = 2 * q[0] * q[0]; + FLT xy = 2 * q[0] * q[1]; + FLT xz = 2 * q[0] * q[2]; + FLT xw = 2 * q[0] * q[3]; - FLT yy = 2 * q[1] * q[1]; - FLT yz = 2 * q[1] * q[2]; - FLT yw = 2 * q[1] * q[3]; + FLT yy = 2 * q[1] * q[1]; + FLT yz = 2 * q[1] * q[2]; + FLT yw = 2 * q[1] * q[3]; - FLT zz = 2 * q[2] * q[2]; - FLT zw = 2 * q[2] * q[3]; + FLT zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; - //opengl major - matrix44[0] = 1 - yy - zz; - matrix44[1] = xy - zw; - matrix44[2] = xz + yw; - matrix44[3] = 0; + //opengl major + matrix44[0] = 1 - yy - zz; + matrix44[1] = xy - zw; + matrix44[2] = xz + yw; + matrix44[3] = 0; - matrix44[4] = xy + zw; - matrix44[5] = 1 - xx - zz; - matrix44[6] = yz - xw; - matrix44[7] = 0; + matrix44[4] = xy + zw; + matrix44[5] = 1 - xx - zz; + matrix44[6] = yz - xw; + matrix44[7] = 0; - matrix44[8] = xz - yw; - matrix44[9] = yz + xw; - matrix44[10] = 1 - xx - yy; - matrix44[11] = 0; + matrix44[8] = xz - yw; + matrix44[9] = yz + xw; + matrix44[10] = 1 - xx - yy; + matrix44[11] = 0; - matrix44[12] = 0; - matrix44[13] = 0; - matrix44[14] = 0; - matrix44[15] = 1; + matrix44[12] = 0; + matrix44[13] = 0; + matrix44[14] = 0; + matrix44[15] = 1; } void quattomatrix33(FLT * matrix33, const FLT * qin) { - FLT q[4]; - quatnormalize(q, qin); + FLT q[4]; + quatnormalize(q, qin); - //Reduced calulation for speed - FLT xx = 2 * q[0] * q[0]; - FLT xy = 2 * q[0] * q[1]; - FLT xz = 2 * q[0] * q[2]; - FLT xw = 2 * q[0] * q[3]; + //Reduced calulation for speed + FLT xx = 2 * q[0] * q[0]; + FLT xy = 2 * q[0] * q[1]; + FLT xz = 2 * q[0] * q[2]; + FLT xw = 2 * q[0] * q[3]; - FLT yy = 2 * q[1] * q[1]; - FLT yz = 2 * q[1] * q[2]; - FLT yw = 2 * q[1] * q[3]; + FLT yy = 2 * q[1] * q[1]; + FLT yz = 2 * q[1] * q[2]; + FLT yw = 2 * q[1] * q[3]; - FLT zz = 2 * q[2] * q[2]; - FLT zw = 2 * q[2] * q[3]; + FLT zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; - //opengl major - matrix33[0] = 1 - yy - zz; - matrix33[1] = xy - zw; - matrix33[2] = xz + yw; + //opengl major + matrix33[0] = 1 - yy - zz; + matrix33[1] = xy - zw; + matrix33[2] = xz + yw; - matrix33[3] = xy + zw; - matrix33[4] = 1 - xx - zz; - matrix33[5] = yz - xw; + matrix33[3] = xy + zw; + matrix33[4] = 1 - xx - zz; + matrix33[5] = yz - xw; - matrix33[6] = xz - yw; - matrix33[7] = yz + xw; - matrix33[8] = 1 - xx - yy; + matrix33[6] = xz - yw; + matrix33[7] = yz + xw; + matrix33[8] = 1 - xx - yy; } void quatgetconjugate(FLT * qout, const FLT * qin) @@ -334,13 +334,13 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) if ( 1 - (cosTheta*cosTheta) <= 0 ) sinTheta = 0; else - sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta)); + sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta)); - FLT Theta = FLT_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 (FLT_FABS(Theta) < DEFAULT_EPSILON) + if (FLT_FABS(Theta) < DEFAULT_EPSILON) quatcopy( q, qa ); - else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) + else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) { quatadd( q, qa, qb ); quatscale( q, q, 0.5 ); @@ -349,8 +349,8 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) { FLT aside[4]; FLT bside[4]; - quatscale( bside, qb, FLT_SIN(t * Theta)); - quatscale( aside, qa, FLT_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, ((FLT)1.)/sinTheta ); } @@ -380,37 +380,47 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) 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]; - } - } + 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; - } - } + 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; + return newMat; } -void rotation_between_vecs_to_m3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) { - FLT q[4]; + FLT q[4]; - getRotationTo(q, v1, v2); + quatfrom2vectors(q, v1, v2); - quattomatrix33(&(m[0][0]), q); + quattomatrix33(&(m->val[0][0]), q); } +void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot) +{ + out[0] = rot.val[0][0] * in[0] + rot.val[1][0] * in[1] + rot.val[2][0] * in[2]; + out[1] = rot.val[0][1] * in[0] + rot.val[1][1] * in[1] + rot.val[2][1] * in[2]; + out[2] = rot.val[0][2] * in[0] + rot.val[1][2] * in[1] + rot.val[2][2] * in[2]; + + return; +} + + // This function based on code from Object-oriented Graphics Rendering Engine // Copyright(c) 2000 - 2012 Torus Knot Software Ltd // under MIT license @@ -423,201 +433,203 @@ If you call this with a dest vector that is close to the inverse of this vector, we will rotate 180 degrees around a generated axis if since in this case ANY axis of rotation is valid. */ -void getRotationTo(FLT *q, const FLT *src, const FLT *dest) -{ - // Based on Stan Melax's article in Game Programming Gems - - // Copy, since cannot modify local - FLT v0[3]; - FLT v1[3]; - normalize3d(v0, src); - normalize3d(v1, dest); - - FLT d = dot3d(v0, v1);// v0.dotProduct(v1); - // If dot == 1, vectors are the same - if (d >= 1.0f) - { - quatsetidentity(q); - return; - } - if (d < (1e-6f - 1.0f)) - { - // Generate an axis - FLT unitX[3] = { 1, 0, 0 }; - FLT unitY[3] = { 0, 1, 0 }; - - FLT axis[3]; - cross3d(axis, unitX, src); // pick an angle - if ((axis[0] < 1.0e-35f) && - (axis[1] < 1.0e-35f) && - (axis[2] < 1.0e-35f)) // pick another if colinear - { - cross3d(axis, unitY, src); - } - normalize3d(axis, axis); - quatfromaxisangle(q, axis, LINMATHPI); - } - else - { - FLT s = FLT_SQRT((1 + d) * 2); - FLT invs = 1 / s; - - FLT c[3]; - cross3d(c, v0, v1); - - q[0] = c[0] * invs; - q[1] = c[1] * invs; - q[2] = c[2] * invs; - q[3] = s * 0.5f; - - quatnormalize(q, q); - } -} - -/////////////////////////////////////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; +void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) +{ + // Based on Stan Melax's article in Game Programming Gems + + // Copy, since cannot modify local + FLT v0[3]; + FLT v1[3]; + normalize3d(v0, src); + normalize3d(v1, dest); + + FLT d = dot3d(v0, v1);// v0.dotProduct(v1); + // If dot == 1, vectors are the same + if (d >= 1.0f) + { + quatsetidentity(q); + return; + } + if (d < (1e-6f - 1.0f)) + { + // Generate an axis + FLT unitX[3] = { 1, 0, 0 }; + FLT unitY[3] = { 0, 1, 0 }; + + FLT axis[3]; + cross3d(axis, unitX, src); // pick an angle + if ((axis[0] < 1.0e-35f) && + (axis[1] < 1.0e-35f) && + (axis[2] < 1.0e-35f)) // pick another if colinear + { + cross3d(axis, unitY, src); + } + normalize3d(axis, axis); + quatfromaxisangle(q, axis, LINMATHPI); + } + else + { + FLT s = FLT_SQRT((1 + d) * 2); + FLT invs = 1 / s; + + FLT c[3]; + //cross3d(c, v0, v1); + cross3d(c, v1, v0); + + q[0] = c[0] * invs; + q[1] = c[1] * invs; + q[2] = c[2] * invs; + q[3] = s * 0.5f; + + quatnormalize(q, q); + } + } +///////////////////////////////////////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 3b11c1b..20a7848 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -87,26 +87,22 @@ 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 ); -void getRotationTo(FLT *q, const FLT *src, const FLT *dest); +void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest); // Matrix Stuff typedef struct { - // row, column, (0,0) in upper left - FLT val[3][3]; + FLT val[3][3]; // row, column } Matrix3x3; +void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot); +void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]); 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_tori/main.c b/tools/lighthousefind_tori/main.c index 14d9423..aa51448 100644 --- a/tools/lighthousefind_tori/main.c +++ b/tools/lighthousefind_tori/main.c @@ -21,52 +21,52 @@ int best_hmd_target = 0; static void printTrackedObject(TrackedObject *to) { - for (unsigned int i = 0; i < to->numSensors; i++) - { - printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n", - i, - to->sensor[i].theta, - to->sensor[i].phi, - to->sensor[i].point.x, - to->sensor[i].point.y, - to->sensor[i].point.z - ); - } + for (unsigned int i = 0; i < to->numSensors; i++) + { + printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n", + i, + to->sensor[i].theta, + to->sensor[i].phi, + to->sensor[i].point.x, + to->sensor[i].point.y, + to->sensor[i].point.z + ); + } } static void runTheNumbers() { - TrackedObject *to; + TrackedObject *to; - to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor))); + to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor))); - int sensorCount = 0; + int sensorCount = 0; - for (int i = 0; i < PTS; i++) - { - // if there are enough valid counts for both the x and y sweeps for sensor i - if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) && - (hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID)) - { - to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0]; - to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1]; - to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2]; - to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0]; - to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1]; - sensorCount++; - } - } + for (int i = 0; i < PTS; i++) + { + // if there are enough valid counts for both the x and y sweeps for sensor i + if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) && + (hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID)) + { + to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0]; + to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1]; + to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2]; + to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0] + LINMATHPI / 2; + to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2; + sensorCount++; + } + } - to->numSensors = sensorCount; + to->numSensors = sensorCount; - printf("Using %d sensors to find lighthouse.\n", sensorCount); + printf("Using %d sensors to find lighthouse.\n", sensorCount); - Point lh = SolveForLighthouse(to, 1); + Point lh = SolveForLighthouse(to, 1); - printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); + printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); - //printTrackedObject(to); - free(to); + //printTrackedObject(to); + free(to); } @@ -74,146 +74,144 @@ int LoadData(char Camera, const char * datafile) { - //First, read the positions of all the sensors on the HMD. - FILE * f = fopen("HMD_points.csv", "r"); - int pt = 0; - if (!f) - { - fprintf(stderr, "error: can't open hmd points.\n"); - return -5; - } - while (!feof(f) && !ferror(f) && pt < PTS) - { - float fx, fy, fz; - int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz); - hmd_points[pt * 3 + 0] = fx; - hmd_points[pt * 3 + 1] = fy; - hmd_points[pt * 3 + 2] = fz; - pt++; - if (r != 3) - { - fprintf(stderr, "Not enough entries on line %d of points\n", pt); - return -8; - } - } - if (pt < PTS) - { - fprintf(stderr, "Not enough points.\n"); - return -9; - } - fclose(f); - printf("Loaded %d points\n", pt); - - //Read all the normals on the HMD into hmd_norms. - f = fopen("HMD_normals.csv", "r"); - int nrm = 0; - if (!f) - { - fprintf(stderr, "error: can't open hmd points.\n"); - return -5; - } - while (!feof(f) && !ferror(f) && nrm < PTS) - { - float fa, fb, fc; - int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc); - hmd_norms[nrm * 3 + 0] = fa; - hmd_norms[nrm * 3 + 1] = fb; - hmd_norms[nrm * 3 + 2] = fc; - nrm++; - if (r != 3) - { - fprintf(stderr, "Not enough entries on line %d of normals\n", nrm); - return -8; - } - } - if (nrm < PTS) - { - fprintf(stderr, "Not enough points.\n"); - return -9; - } - if (nrm != pt) - { - fprintf(stderr, "point/normal counts disagree.\n"); - return -9; - } - fclose(f); - printf("Loaded %d norms\n", nrm); - - //Actually load the processed data! - int xck = 0; - f = fopen(datafile, "r"); - if (!f) - { - fprintf(stderr, "Error: cannot open %s\n", datafile); - exit(-11); - } - int lineno = 0; - while (!feof(f)) - { - //Format: - // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936 - lineno++; - char devn[10]; - char inn[10]; - int id; - int pointct; - double avgTime; - double avgLen; - double stddevTime; - double stddevLen; - int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen); - if (ct == 0) continue; - if (ct != 8) - { - fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno); - } - if (strcmp(devn, "HMD") != 0) continue; - - if (inn[0] != Camera) continue; - - int isy = inn[1] == 'Y'; - -// hmd_point_angles[id * 2 + isy] = (avgTime - 200000) / 200000 * 3.1415926535 / 2.0; - hmd_point_angles[id * 2 + isy] = (FLT)(avgTime / 48000000 * 60 * M_PI * 2); - - hmd_point_counts[id * 2 + isy] = pointct; - } - fclose(f); - - - int targpd; - int maxhits = 0; - - for (targpd = 0; targpd < PTS; targpd++) - { - int hits = hmd_point_counts[targpd * 2 + 0]; - if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1]; - //Need an X and a Y lock. - - if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; } - } - if (maxhits < MIN_HITS_FOR_VALID) - { - fprintf(stderr, "Error: Not enough data for a primary fix.\n"); - } - - return 0; + //First, read the positions of all the sensors on the HMD. + FILE * f = fopen("HMD_points.csv", "r"); + int pt = 0; + if (!f) + { + fprintf(stderr, "error: can't open hmd points.\n"); + return -5; + } + while (!feof(f) && !ferror(f) && pt < PTS) + { + float fx, fy, fz; + int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz); + hmd_points[pt * 3 + 0] = fx; + hmd_points[pt * 3 + 1] = fy; + hmd_points[pt * 3 + 2] = fz; + pt++; + if (r != 3) + { + fprintf(stderr, "Not enough entries on line %d of points\n", pt); + return -8; + } + } + if (pt < PTS) + { + fprintf(stderr, "Not enough points.\n"); + return -9; + } + fclose(f); + printf("Loaded %d points\n", pt); + + //Read all the normals on the HMD into hmd_norms. + f = fopen("HMD_normals.csv", "r"); + int nrm = 0; + if (!f) + { + fprintf(stderr, "error: can't open hmd points.\n"); + return -5; + } + while (!feof(f) && !ferror(f) && nrm < PTS) + { + float fa, fb, fc; + int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc); + hmd_norms[nrm * 3 + 0] = fa; + hmd_norms[nrm * 3 + 1] = fb; + hmd_norms[nrm * 3 + 2] = fc; + nrm++; + if (r != 3) + { + fprintf(stderr, "Not enough entries on line %d of normals\n", nrm); + return -8; + } + } + if (nrm < PTS) + { + fprintf(stderr, "Not enough points.\n"); + return -9; + } + if (nrm != pt) + { + fprintf(stderr, "point/normal counts disagree.\n"); + return -9; + } + fclose(f); + printf("Loaded %d norms\n", nrm); + + //Actually load the processed data! + int xck = 0; + f = fopen(datafile, "r"); + if (!f) + { + fprintf(stderr, "Error: cannot open %s\n", datafile); + exit(-11); + } + int lineno = 0; + while (!feof(f)) + { + //Format: + // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936 + lineno++; + char devn[10]; + char inn[10]; + int id; + int pointct; + double avgTime; + double avgLen; + double stddevTime; + double stddevLen; + int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen); + if (ct == 0) continue; + if (ct != 8) + { + fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno); + } + if (strcmp(devn, "HMD") != 0) continue; + + if (inn[0] != Camera) continue; + + int isy = inn[1] == 'Y'; + + hmd_point_angles[id * 2 + isy] = ((FLT)avgTime - 200000) * LINMATHPI / 200000 / 2; + + hmd_point_counts[id * 2 + isy] = pointct; + } + fclose(f); + + + int targpd; + int maxhits = 0; + + for (targpd = 0; targpd < PTS; targpd++) + { + int hits = hmd_point_counts[targpd * 2 + 0]; + if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1]; + //Need an X and a Y lock. + + if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; } + } + if (maxhits < MIN_HITS_FOR_VALID) + { + fprintf(stderr, "Error: Not enough data for a primary fix.\n"); + } + + return 0; } int main(int argc, char ** argv) { - if (argc != 3) - { - fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n"); - exit(-1); - } + if (argc != 3) + { + fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n"); + exit(-1); + } - //Load either 'L' (LH1) or 'R' (LH2) data. - if (LoadData(argv[1][0], argv[2])) return 5; + //Load either 'L' (LH1) or 'R' (LH2) data. + if (LoadData(argv[1][0], argv[2])) return 5; + runTheNumbers(); - runTheNumbers(); - - return 0; + return 0; } diff --git a/tools/lighthousefind_tori/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h index 51cd04f..4cfbcdc 100644 --- a/tools/lighthousefind_tori/tori_includes.h +++ b/tools/lighthousefind_tori/tori_includes.h @@ -8,23 +8,23 @@ typedef struct { - double x; - double y; - double z; + double x; + double y; + double z; } Point; typedef struct { - Point point; // location of the sensor on the tracked object; - Point normal; // unit vector indicating the normal for the sensor - double theta; // "horizontal" angular measurement from lighthouse radians - double phi; // "vertical" angular measurement from lighthouse in radians. + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. } TrackedSensor; typedef struct { - size_t numSensors; - TrackedSensor sensor[0]; + size_t numSensors; + TrackedSensor sensor[0]; } TrackedObject; @@ -36,15 +36,14 @@ typedef struct typedef union { - struct - { - unsigned char Blue; - unsigned char Green; - unsigned char Red; - unsigned char Alpha; - }; -// float float_value; - uint32_t long_value; + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + uint32_t long_value; } RGBValue; static RGBValue RED = { .Red = 255, .Green = 0, .Blue = 0, .Alpha = 125 }; @@ -56,10 +55,6 @@ static const double WORLD_BOUNDS = 100; static const float DefaultPointsPerOuterDiameter = 60; - - - - //#define TORI_DEBUG #endif diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c index 43dd9a2..22a0ce2 100644 --- a/tools/lighthousefind_tori/torus_localizer.c +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -9,201 +9,197 @@ 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); + 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; - FLT v1[3] = { 0, 0, 1 }; - FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; + Matrix3x3 result; + FLT v1[3] = { 0, 0, 1 }; + FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; - normalize_v3(v2); + normalize3d(v2,v2); - rotation_between_vecs_to_mat3(result.val, v1, v2); + rotation_between_vecs_to_m3(&result, v1, v2); - FLT result2[9]; + // Useful for debugging... + FLT v2b[3]; + rotate_vec(v2b, v1, result); - 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; + return result; } +//void TestGetRotationMatrixForTorus() +//{ +// // a={x=0.079967096447944641 y=0.045225199311971664 z=0.034787099808454514 } +// // b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 } +// +// // a={x=0.050822000950574875 y=0.052537899464368820 z=0.033285100013017654 } +// // b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 } +// +//} + Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) { - Point q; + Point q; - double pf[3] = { p.x, p.y, p.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; + double pf[3] = { p.x, p.y, p.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; - return q; + return q; } double angleFromPoints(Point p1, Point p2, Point center) { - Point v1, v2, v1norm, v2norm; - v1.x = p1.x - center.x; - v1.y = p1.y - center.y; - v1.z = p1.z - center.z; + Point v1, v2, v1norm, v2norm; + v1.x = p1.x - center.x; + v1.y = p1.y - center.y; + v1.z = p1.z - center.z; - v2.x = p2.x - center.x; - v2.y = p2.y - center.y; - v2.z = p2.z - center.z; + v2.x = p2.x - center.x; + v2.y = p2.y - center.y; + v2.z = p2.z - center.z; - double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); - v1norm.x = v1.x / v1mag; - v1norm.y = v1.y / v1mag; - v1norm.z = v1.z / v1mag; + double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); + v1norm.x = v1.x / v1mag; + v1norm.y = v1.y / v1mag; + v1norm.z = v1.z / v1mag; - double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); - v2norm.x = v2.x / v2mag; - v2norm.y = v2.y / v2mag; - v2norm.z = v2.z / v2mag; + double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); + v2norm.x = v2.x / v2mag; + v2norm.y = v2.y / v2mag; + v2norm.z = v2.z / v2mag; - double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; + double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; - double angle = acos(res); + double angle = acos(res); - return angle; + return angle; } Point midpoint(Point a, Point b) { - Point m; - m.x = (a.x + b.x) / 2; - m.y = (a.y + b.y) / 2; - m.z = (a.z + b.z) / 2; + Point m; + m.x = (a.x + b.x) / 2; + m.y = (a.y + b.y) / 2; + m.z = (a.z + b.z) / 2; - return m; + return m; } -// This is the second incarnation of the torus generator. It is intended to differ from the initial torus generator by -// producing a point cloud of a torus where the points density is more uniform across the torus. This will allow -// us to be more efficient in finding a solution. +// This torus generator creates a point cloud of the given torus, and attempts to keep the +// density of the points fairly uniform across the surface of the torus. void partialTorusGenerator( - Point p1, - Point p2, - double toroidalStartAngle, - double toroidalEndAngle, - double poloidalStartAngle, - double poloidalEndAngle, - double lighthouseAngle, - double toroidalPrecision, - Point **pointCloud) + Point p1, + Point p2, + double toroidalStartAngle, + double toroidalEndAngle, + double poloidalStartAngle, + double poloidalEndAngle, + double lighthouseAngle, + double toroidalPrecision, + Point **pointCloud) { - double poloidalRadius = 0; - double toroidalRadius = 0; + double poloidalRadius = 0; + double toroidalRadius = 0; - Point m = midpoint(p1, p2); - double distanceBetweenPoints = distance(p1, p2); + Point m = midpoint(p1, p2); + double distanceBetweenPoints = distance(p1, p2); - // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally - // thinking the tori converge at the location of the tracked object instead of at the lighthouse. - double centralAngleToIgnore = lighthouseAngle * 3; + // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally + // thinking the tori converge at the location of the tracked object instead of at the lighthouse. + double centralAngleToIgnore = lighthouseAngle * 3; - Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); + Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); - toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); + toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); - poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); + poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); - double poloidalPrecision = M_PI * 2 / toroidalPrecision; + double poloidalPrecision = M_PI * 2 / toroidalPrecision; - //unsigned int pointCount = toroidalPrecision * toroidalPrecision / 2 * (toroidalEndAngle - toroidalStartAngle) / (M_PI * 2) * (poloidalEndAngle - poloidalStartAngle) / (M_PI * 1); - //unsigned int pointCount = (unsigned int)(toroidalPrecision * ((M_PI - lighthouseAngle) * 2 / poloidalPrecision + 1) + 1); - // TODO: This calculation of the number of points that we will generate is excessively large (probably by about a factor of 2 or more) We can do better. - //float pointEstimate = (pointCount + 1000) * sizeof(Point) * 2 * M_PI / (toroidalEndAngle - toroidalStartAngle); - unsigned int pointCount = 0; + unsigned int pointCount = 0; - for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) - { - // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle - // We do this so our point cloud will have a more even distribution of points across the surface of the torus. - double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; + // This loop tries to (cheaply) figure out an upper bound on the number of points we'll have in our point cloud + for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) + { + // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle + // We do this so our point cloud will have a more even distribution of points across the surface of the torus. + double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; - double step_distance = 2 * M_PI / steps; + double step_distance = 2 * M_PI / steps; - pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2); - } + pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2); + } - *pointCloud = malloc(pointCount * sizeof(Point) ); + *pointCloud = malloc(pointCount * sizeof(Point) ); - assert(0 != *pointCloud); + assert(0 != *pointCloud); - (*pointCloud)[pointCount - 1].x = -1000; - (*pointCloud)[pointCount - 1].y = -1000; - (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now. + (*pointCloud)[pointCount - 1].x = -1000; + (*pointCloud)[pointCount - 1].y = -1000; + (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now. - size_t currentPoint = 0; + size_t currentPoint = 0; - for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) - { - // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle - // We do this so our point cloud will have a more even distribution of points across the surface of the torus. - double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; + for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) + { + // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle + // We do this so our point cloud will have a more even distribution of points across the surface of the torus. + double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; - double step_distance = 2 * M_PI / steps; + double step_distance = 2 * M_PI / steps; - //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) - for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance) - { - if (currentPoint >= pointCount - 1) - { - int a = 0; - } - assert(currentPoint < pointCount - 1); - (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); - (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); - (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep); - (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m); + //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) + for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance) + { + if (currentPoint >= pointCount - 1) + { + int a = 0; + } + assert(currentPoint < pointCount - 1); + (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); + (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); + (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep); + (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m); - // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up - // and hence we know that nothing with a negative z value is a possible lightouse location. - // Before this code can go live, we'll have to take the normals into account and remove this hack. - if ((*pointCloud)[currentPoint].z > 0) - { - currentPoint++; - } - } - } + // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up + // and hence we know that nothing with a negative z value is a possible lightouse location. + // Before this code can go live, we'll have to take the normals into account and remove this hack. + if ((*pointCloud)[currentPoint].z > 0) + { + currentPoint++; + } + } + } #ifdef TORI_DEBUG - printf("%d / %d\n", currentPoint, pointCount); + printf("%d / %d\n", currentPoint, pointCount); #endif - (*pointCloud)[currentPoint].x = -1000; - (*pointCloud)[currentPoint].y = -1000; - (*pointCloud)[currentPoint].z = -1000; + (*pointCloud)[currentPoint].x = -1000; + (*pointCloud)[currentPoint].y = -1000; + (*pointCloud)[currentPoint].z = -1000; } void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointCloud) { - double centralAngleToIgnore = lighthouseAngle * 6; + double centralAngleToIgnore = lighthouseAngle * 6; - centralAngleToIgnore = 20.0 / 180.0 * M_PI; + centralAngleToIgnore = 20.0 / 180.0 * M_PI; - partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud); + partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud); - return; + return; } @@ -218,163 +214,163 @@ 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, - Point point, - double *toroidalAngle, - double *poloidalAngle) + Point torusP1, + Point torusP2, + double lighthouseAngle, + Point point, + double *toroidalAngle, + double *poloidalAngle) { - // 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); - - // 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. - // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. - rot = inverseM33(rot); - Point origin; - origin.x = 0; - origin.y = 0; - origin.z = 0; - - Point m = midpoint(torusP1, torusP2); - - // 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 - Point pointF; - - // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also - // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system - // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. - pointF.x = point.x - m.x; - pointF.y = point.y - m.y; - pointF.z = point.z - m.z; - - // now we'll undo the rotation part. - pointF = RotateAndTranslatePoint(pointF, rot, origin); - - // hooray, now pointF is in our more-friendly coordinate system. - - // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. - // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the - // angle between a vector to pointF and a vector along the x axis. - - *toroidalAngle = atan(pointF.y / pointF.x); - if (pointF.x < 0) - { - *toroidalAngle += M_PI; - } - - // SCORE!! We've got the toroidal angle. We're half done! - - // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to - // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle - // so that the point we're focusin on will lie on the x/z plane. We then should translate the - // torus so that the center of the poloidal circle is at the origin. At that point, it will - // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a - // vector from the origin to the point. - - // okay, instead of rotating the torus & point by the toroidal angle to get the point on - // the xz plane, we're going to take advantage of the radial symmetry of the torus - // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't - // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point - // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent - // this new coordinate system - - Point pointG; - pointG.z = pointF.z; - pointG.y = 0; - pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); - - // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, - // our next step will be to shift it down so that the center of the poloidal circle is at the origin. - // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat - // 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 poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); - - // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. - // The shift along the X axis will be the toroidal radius. - - Point pointH; - pointH.z = pointG.z; - pointH.y = pointG.y; - pointH.x = pointG.x - toroidalRadius; - - // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, - // that will be the poloidal angle we're looking for. (crosses fingers) - - *poloidalAngle = atan(pointH.z / pointH.x); - if (pointH.x < 0) - { - *poloidalAngle += M_PI; - } - - // Wow, that ended up being not so much code, but a lot of interesting trig. - // can't remember the last time I spent so much time working through each line of code. - - return; + // 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); + + // 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. + // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. + rot = inverseM33(rot); + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(torusP1, torusP2); + + // 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 + Point pointF; + + // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also + // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system + // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. + pointF.x = point.x - m.x; + pointF.y = point.y - m.y; + pointF.z = point.z - m.z; + + // now we'll undo the rotation part. + pointF = RotateAndTranslatePoint(pointF, rot, origin); + + // hooray, now pointF is in our more-friendly coordinate system. + + // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. + // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the + // angle between a vector to pointF and a vector along the x axis. + + *toroidalAngle = atan(pointF.y / pointF.x); + if (pointF.x < 0) + { + *toroidalAngle += M_PI; + } + + // SCORE!! We've got the toroidal angle. We're half done! + + // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to + // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle + // so that the point we're focusin on will lie on the x/z plane. We then should translate the + // torus so that the center of the poloidal circle is at the origin. At that point, it will + // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a + // vector from the origin to the point. + + // okay, instead of rotating the torus & point by the toroidal angle to get the point on + // the xz plane, we're going to take advantage of the radial symmetry of the torus + // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't + // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point + // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent + // this new coordinate system + + Point pointG; + pointG.z = pointF.z; + pointG.y = 0; + pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); + + // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, + // our next step will be to shift it down so that the center of the poloidal circle is at the origin. + // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat + // 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 poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); + + // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. + // The shift along the X axis will be the toroidal radius. + + Point pointH; + pointH.z = pointG.z; + pointH.y = pointG.y; + pointH.x = pointG.x - toroidalRadius; + + // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, + // that will be the poloidal angle we're looking for. (crosses fingers) + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + // Wow, that ended up being not so much code, but a lot of interesting trig. + // can't remember the last time I spent so much time working through each line of code. + + return; } double FindSmallestDistance(Point p, Point* cloud) { - Point *cp = cloud; - double smallestDistance = 10000000000000.0; - - while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) - { - double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z)); - if (distance < smallestDistance) - { - smallestDistance = distance; - } - cp++; - } - smallestDistance = sqrt(smallestDistance); - return smallestDistance; + Point *cp = cloud; + double smallestDistance = 10000000000000.0; + + while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) + { + double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z)); + if (distance < smallestDistance) + { + smallestDistance = distance; + } + cp++; + } + smallestDistance = sqrt(smallestDistance); + return smallestDistance; } // Given a cloud and a list of clouds, find the point on masterCloud that best matches clouds. Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds) { - Point bestMatch = { 0 }; - double bestDistance = 10000000000000.0; - Point *cp = masterCloud; - int point = 0; - while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) - { - point++; + Point bestMatch = { 0 }; + double bestDistance = 10000000000000.0; + Point *cp = masterCloud; + int point = 0; + while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) + { + point++; #ifdef TORI_DEBUG - if (point % 100 == 0) - { - printf("."); - } + if (point % 100 == 0) + { + printf("."); + } #endif - double currentDistance = 0; - for (int i = 0; i < numClouds; i++) - { - if (clouds[i] == masterCloud) - { - continue; - } - Point* cloud = clouds[i]; - currentDistance += FindSmallestDistance(*cp, cloud); - } - - if (currentDistance < bestDistance) - { - bestDistance = currentDistance; - bestMatch = *cp; - } - cp++; - } - - return bestMatch; + double currentDistance = 0; + for (int i = 0; i < numClouds; i++) + { + if (clouds[i] == masterCloud) + { + continue; + } + Point* cloud = clouds[i]; + currentDistance += FindSmallestDistance(*cp, cloud); + } + + if (currentDistance < bestDistance) + { + bestDistance = currentDistance; + bestMatch = *cp; + } + cp++; + } + + return bestMatch; } @@ -382,197 +378,215 @@ Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds) typedef struct { - Point a; - Point b; - double angle; + 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)); - double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta)); + double angle = acos(cos(a->phi - b->phi)*cos(a->theta - b->theta)); + double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta)); - return angle; + return angle; } double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) { - double p = (a->phi - b->phi); - double d = (a->theta - b->theta); + double p = (a->phi - b->phi); + double d = (a->theta - b->theta); - double adjd = sin((a->phi + b->phi) / 2); - double adjP = sin((a->theta + b->theta) / 2); - double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); - return pythAngle; + double adjd = sin((a->phi + b->phi) / 2); + double adjP = sin((a->theta + b->theta) / 2); + double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; } Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { - PointsAndAngle pna[MAX_POINT_PAIRS]; - //Point lh = { 10, 0, 200 }; - - size_t pnaCount = 0; - for (unsigned int i = 0; i < obj->numSensors; i++) - { - for (unsigned int j = 0; j < i; j++) - { - if (pnaCount < MAX_POINT_PAIRS) - { - pna[pnaCount].a = obj->sensor[i].point; - pna[pnaCount].b = obj->sensor[j].point; - - pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); - - double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); - - //double tmp = angleFromPoints(pna[pnaCount].a, pna[pnaCount].b, lh); - - pnaCount++; - } - } - } - - //Point **pointCloud = malloc(sizeof(Point*)* pnaCount); - Point **pointCloud = malloc(sizeof(void*)* pnaCount); - - FILE *f = NULL; - if (doLogOutput) - { - f = fopen("pointcloud2.pcd", "wb"); - writePcdHeader(f); - writeAxes(f); - } - - for (unsigned int i = 0; i < pnaCount; i++) - { - torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i])); - if (doLogOutput) - { - writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]); - } - - } - - Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount); - - if (doLogOutput) - { - markPointWithStar(f, bestMatchA, 0xFF0000); - } + PointsAndAngle pna[MAX_POINT_PAIRS]; + + size_t pnaCount = 0; + for (unsigned int i = 0; i < obj->numSensors; i++) + { + for (unsigned int j = 0; j < i; j++) + { + if (pnaCount < MAX_POINT_PAIRS) + { + pna[pnaCount].a = obj->sensor[i].point; + pna[pnaCount].b = obj->sensor[j].point; + + pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + pnaCount++; + } + } + } + + Point **pointCloud = malloc(sizeof(void*)* pnaCount); + + FILE *f = NULL; + if (doLogOutput) + { + f = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(f); + writeAxes(f); + } + + for (unsigned int i = 0; i < pnaCount; i++) + { + torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i])); + if (doLogOutput) + { + writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]); + } + + } + + Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount); + + for (unsigned int i = 0; i < pnaCount; i++) + { + free(pointCloud[i]); + pointCloud[i] = NULL; + } + + if (doLogOutput) + { + markPointWithStar(f, bestMatchA, 0xFF0000); + } #ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z); + printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z); #endif - // Now, let's add an extra patch or torus near the point we just found. + // Now, let's add an extra patch or torus near the point we just found. + + double toroidalAngle = 0; + double poloidalAngle = 0; - double toroidalAngle = 0; - double poloidalAngle = 0; + Point **pointCloud2 = malloc(sizeof(void*)* pnaCount); - Point **pointCloud2 = malloc(sizeof(void*)* pnaCount); + for (unsigned int i = 0; i < pnaCount; i++) + { + estimateToroidalAndPoloidalAngleOfPoint( + pna[i].a, + pna[i].b, + pna[i].angle, + bestMatchA, + &toroidalAngle, + &poloidalAngle); - for (unsigned int i = 0; i < pnaCount; i++) - { - estimateToroidalAndPoloidalAngleOfPoint( - pna[i].a, - pna[i].b, - pna[i].angle, - bestMatchA, - &toroidalAngle, - &poloidalAngle); + partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.1, toroidalAngle + 0.1, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i])); - partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.2, toroidalAngle + 0.2, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i])); + if (doLogOutput) + { + writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]); + } - if (doLogOutput) - { - writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]); - } + } - } + Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount); - Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount); - if (doLogOutput) - { - markPointWithStar(f, bestMatchB, 0x00FF00); - } + for (unsigned int i = 0; i < pnaCount; i++) + { + free(pointCloud2[i]); + pointCloud2[i] = NULL; + } + + if (doLogOutput) + { + markPointWithStar(f, bestMatchB, 0x00FF00); + } #ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z); + printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z); #endif - Point **pointCloud3 = malloc(sizeof(void*)* pnaCount); + Point **pointCloud3 = malloc(sizeof(void*)* pnaCount); + + for (unsigned int i = 0; i < pnaCount; i++) + { + estimateToroidalAndPoloidalAngleOfPoint( + pna[i].a, + pna[i].b, + pna[i].angle, + bestMatchB, + &toroidalAngle, + &poloidalAngle); + + partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.1, poloidalAngle + 0.1, pna[i].angle, 3000, &(pointCloud3[i])); - for (unsigned int i = 0; i < pnaCount; i++) - { - estimateToroidalAndPoloidalAngleOfPoint( - pna[i].a, - pna[i].b, - pna[i].angle, - bestMatchB, - &toroidalAngle, - &poloidalAngle); + if (doLogOutput) + { + writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]); + } - partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.05, poloidalAngle + 0.05, pna[i].angle, 3000, &(pointCloud3[i])); + } - if (doLogOutput) - { - writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]); - } + Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount); - } + for (unsigned int i = 0; i < pnaCount; i++) + { + free(pointCloud3[i]); + pointCloud3[i] = NULL; + } - Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount); - if (doLogOutput) - { - markPointWithStar(f, bestMatchC, 0xFFFFFF); - } + if (doLogOutput) + { + markPointWithStar(f, bestMatchC, 0xFFFFFF); + } #ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z); + printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z); #endif - if (doLogOutput) - { - updateHeader(f); - fclose(f); - } + if (doLogOutput) + { + updateHeader(f); + fclose(f); + } + + - return bestMatchC; + return bestMatchC; } static Point makeUnitPoint(Point *p) { - Point newP; - double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z); - newP.x = p->x / r; - newP.y = p->y / r; - newP.z = p->z / r; + Point newP; + double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z); + newP.x = p->x / r; + newP.y = p->y / r; + newP.z = p->z / r; - return newP; + return newP; } static double getPhi(Point p) { - // double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z))); - // double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z); - double phi = atan(p.x / p.z); - return phi; + // double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z))); + // double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z); + double phi = atan(p.x / p.z); + return phi; } static double getTheta(Point p) { - //double theta = atan(p.y / p.x); - double theta = atan(p.x / p.y); - return theta; + //double theta = atan(p.y / p.x); + double theta = atan(p.x / p.y); + return theta; } // subtraction static Point PointSub(Point a, Point b) { - Point newPoint; + Point newPoint; - newPoint.x = a.x - b.x; - newPoint.y = a.y - b.y; - newPoint.z = a.z - b.z; + newPoint.x = a.x - b.x; + newPoint.y = a.y - b.y; + newPoint.z = a.z - b.z; - return newPoint; + return newPoint; } diff --git a/tools/lighthousefind_tori/visualization.c b/tools/lighthousefind_tori/visualization.c index d348c2d..12cbfee 100644 --- a/tools/lighthousefind_tori/visualization.c +++ b/tools/lighthousefind_tori/visualization.c @@ -6,89 +6,89 @@ int pointsWritten = 0; void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) { - fprintf(file, "%f %f %f %u\n", x, y, z, rgb); - pointsWritten++; + fprintf(file, "%f %f %f %u\n", x, y, z, rgb); + pointsWritten++; } void updateHeader(FILE * file) { - fseek(file, 0x4C, SEEK_SET); - fprintf(file, "%d", pointsWritten); - fseek(file, 0x7C, SEEK_SET); - fprintf(file, "%d", pointsWritten); + fseek(file, 0x4C, SEEK_SET); + fprintf(file, "%d", pointsWritten); + fseek(file, 0x7C, SEEK_SET); + fprintf(file, "%d", pointsWritten); } void writeAxes(FILE * file) { - double scale = 5; - for (double i = 0; i < scale; i = i + scale / 1000) - { - writePoint(file, i, 0, 0, 255); - } - for (double i = 0; i < scale; i = i + scale / 1000) - { - if ((int)(i / (scale / 5)) % 2 == 1) - { - writePoint(file, 0, i, 0, 255 << 8); - } - } - for (double i = 0; i < scale; i = i + scale / 10001) - { - if ((int)(i / (scale / 10)) % 2 == 1) - { - writePoint(file, 0, 0, i, 255 << 16); - } - } + double scale = 5; + for (double i = 0; i < scale; i = i + scale / 1000) + { + writePoint(file, i, 0, 0, 255); + } + for (double i = 0; i < scale; i = i + scale / 1000) + { + if ((int)(i / (scale / 5)) % 2 == 1) + { + writePoint(file, 0, i, 0, 255 << 8); + } + } + for (double i = 0; i < scale; i = i + scale / 10001) + { + if ((int)(i / (scale / 10)) % 2 == 1) + { + writePoint(file, 0, 0, i, 255 << 16); + } + } } void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) { - int max = 50; - for (int i = 0; i < max; i++) - { - writePoint(file, - (a.x*i + b.x*(max - i)) / max, - (a.y*i + b.y*(max - i)) / max, - (a.z*i + b.z*(max - i)) / max, - color); - } + int max = 50; + for (int i = 0; i < max; i++) + { + writePoint(file, + (a.x*i + b.x*(max - i)) / max, + (a.y*i + b.y*(max - i)) / max, + (a.z*i + b.z*(max - i)) / max, + color); + } } void writePcdHeader(FILE * file) { - fprintf(file, "VERSION 0.7\n"); - fprintf(file, "FIELDS x y z rgb\n"); - fprintf(file, "SIZE 4 4 4 4\n"); - fprintf(file, "TYPE F F F U\n"); - fprintf(file, "COUNT 1 1 1 1\n"); - fprintf(file, "WIDTH \n"); - fprintf(file, "HEIGHT 1\n"); - fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n"); - fprintf(file, "POINTS \n"); - fprintf(file, "DATA ascii\n"); + fprintf(file, "VERSION 0.7\n"); + fprintf(file, "FIELDS x y z rgb\n"); + fprintf(file, "SIZE 4 4 4 4\n"); + fprintf(file, "TYPE F F F U\n"); + fprintf(file, "COUNT 1 1 1 1\n"); + fprintf(file, "WIDTH \n"); + fprintf(file, "HEIGHT 1\n"); + fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n"); + fprintf(file, "POINTS \n"); + fprintf(file, "DATA ascii\n"); - //fprintf(file, "100000.0, 100000.0, 100000\n"); + //fprintf(file, "100000.0, 100000.0, 100000\n"); } void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) { - Point *currentPoint = pointCloud; + Point *currentPoint = pointCloud; - while (currentPoint->x != -1000 || currentPoint->y != -1000 || currentPoint->z != -1000) - { - writePoint(f, currentPoint->x, currentPoint->y, currentPoint->z, Color); - currentPoint++; - } + while (currentPoint->x != -1000 || currentPoint->y != -1000 || currentPoint->z != -1000) + { + writePoint(f, currentPoint->x, currentPoint->y, currentPoint->z, Color); + currentPoint++; + } } void markPointWithStar(FILE *file, Point point, unsigned int color) { - double i; - for (i = -0.8; i <= 0.8; i = i + 0.0025) - { - writePoint(file, point.x + i, point.y, point.z, color); - writePoint(file, point.x, point.y + i, point.z, color); - writePoint(file, point.x, point.y, point.z + i, color); - } + double i; + for (i = -0.8; i <= 0.8; i = i + 0.0025) + { + writePoint(file, point.x + i, point.y, point.z, color); + writePoint(file, point.x, point.y + i, point.z, color); + writePoint(file, point.x, point.y, point.z + i, color); + } } diff --git a/tools/lighthousefind_tori/visualization.h b/tools/lighthousefind_tori/visualization.h index f0263eb..da69ed2 100644 --- a/tools/lighthousefind_tori/visualization.h +++ b/tools/lighthousefind_tori/visualization.h @@ -23,24 +23,24 @@ void markPointWithStar(FILE *file, Point point, unsigned int color); #define MAX_COLORS 18 static unsigned int COLORS[] = { - 0x00FFFF, - 0xFF00FF, - 0xFFFF00, - 0xFF0000, - 0x00FF00, - 0x0000FF, - 0x0080FF, - 0x8000FF, - 0x80FF00, - 0x00FF80, - 0xFF0080, - 0xFF8000, - 0x008080, - 0x800080, - 0x808000, - 0x000080, - 0x008000, - 0x800000 + 0x00FFFF, + 0xFF00FF, + 0xFFFF00, + 0xFF0000, + 0x00FF00, + 0x0000FF, + 0x0080FF, + 0x8000FF, + 0x80FF00, + 0x00FF80, + 0xFF0080, + 0xFF8000, + 0x008080, + 0x800080, + 0x808000, + 0x000080, + 0x008000, + 0x800000 }; #endif -- cgit v1.2.3