Add to_string to KDTree for debug printing

That earlier change introduced an error because YAML sucks

.clang-format

@@ -8,5 +8,6 @@ AllowShortLambdasOnASingleLine: Inline
 AllowShortLoopsOnASingleLine: 'false'
 AlwaysBreakBeforeMultilineStrings: 'true'
 IndentWidth: '4'
+ColumnLimit: 100
 
 ...

kdtree.h

@@ -0,0 +1,120 @@
+#include <algorithm>
+#include <glm/glm.hpp>
+#include <string>
+#include <vector>
+
+// Forward declarations
+struct Node;
+struct Point;
+struct Triangle;
+
+struct Node {
+    Node(int axis, Point *point, Node *left, Node *right)
+        : axis(axis), point(point), left(left), right(right) {}
+
+    int axis;
+
+    Point *point;
+
+    Node *left;
+    Node *right;
+};
+
+struct Triangle {
+    Triangle(Point *p1, Point *p2, Point *p3) : p1(p1), p2(p2), p3(p3) {}
+
+    Point *p1;
+    Point *p2;
+    Point *p3;
+};
+
+struct Point {
+    Point(float coordinates[3], Triangle *triangle)
+        : coordinates(coordinates), triangle(triangle) {}
+
+    float *coordinates;
+
+    Triangle *triangle;
+};
+
+class KDTree {
+  public:
+    KDTree(std::vector<Point *> points) { root = build(points, 0); }
+
+    ~KDTree() = default;
+
+    std::string to_string() {
+        std::string str = "";
+        to_string_recurse(str, root, 0);
+        return str;
+    }
+
+  private:
+    Node *root;
+
+    int MAX_DEPTH = 500;
+
+    // Returns a comparator lambda for assessing which of the two points has a
+    // greater coordinate in the given axis.
+    auto get_point_comparator(int axis) {
+        return [axis](Point *p1, Point *p2) {
+            return p1->coordinates[axis] < p2->coordinates[axis];
+        };
+    }
+
+    Node *build(std::vector<Point *> points, int depth) {
+        // Exit conditions
+        if (points.empty() || depth > MAX_DEPTH) { return nullptr; }
+
+        // Select axis by choosing the one with maximal extent
+        float max_extent = 0;
+        int axis = 0;
+
+        for (int it_axis = 0; it_axis < 3; it_axis++) {
+            // Get extent along this axis
+            auto comparator = get_point_comparator(it_axis);
+
+            Point *min = *std::max_element(points.begin(), points.end(), comparator);
+            Point *max = *std::max_element(points.begin(), points.end(), comparator);
+
+            float extent = max->coordinates[it_axis] - min->coordinates[it_axis];
+
+            // Is it greater than max_extent?
+            if (extent > max_extent) {
+                // If so, make this the splitting axis
+                max_extent = extent;
+                axis = it_axis;
+            }
+        }
+
+        // Choose the median as the pivot and sort the points into
+        // left-of-median and right-of-median using nth_element
+        int middle = points.size() / 2;
+
+        std::nth_element(points.begin(), points.begin() + middle, points.end(),
+                         get_point_comparator(axis));
+
+        Point *median = points[middle];
+
+        // TODO: This copies. Can we split the vector into two without copying?
+        std::vector<Point *> left_of_median(points.begin(), points.begin() + middle);
+        std::vector<Point *> right_of_median(points.begin() + middle, points.end());
+
+        // Create node, recursively call to construct subtree
+        return new Node(axis, median, build(left_of_median, depth + 1),
+                        build(right_of_median, depth + 1));
+    }
+
+    void to_string_recurse(std::string &str, Node *node, int depth) {
+        if (node == nullptr) { return; }
+
+        Point *point = node->point;
+
+        str += std::string(depth, '-') + std::to_string(point->coordinates[0]) + ", " +
+               std::to_string(point->coordinates[1]) + ", " +
+               std::to_string(point->coordinates[2]) + "\n";
+
+        to_string_recurse(str, node->left, depth + 1);
+        to_string_recurse(str, node->right, depth + 1);
+    }
+};