Adding lighthousefind_tori

1 files changed, 94 insertions, 0 deletions

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);
+    }
+
+}