Rough cout implementation of ray intersection order

The order in which nodes are checked seems to be correct

kdtree.h

@@ -1,5 +1,6 @@
 #include <algorithm>
 #include <glm/glm.hpp>
+#include <iostream>
 #include <string>
 #include <vector>
 
@@ -32,6 +33,19 @@ struct Point {
     Point(float coordinates[3], Triangle *triangle)
         : coordinates(coordinates), triangle(triangle) {}
 
+    Point operator+(const Point &other) const {
+        return Point(new float[3]{coordinates[0] + other.coordinates[0],
+                                  coordinates[1] + other.coordinates[1],
+                                  coordinates[2] + other.coordinates[2]},
+                     nullptr);
+    }
+
+    Point operator*(float scalar) const {
+        return Point(
+            new float[3]{coordinates[0] * scalar, coordinates[1] * scalar, coordinates[2] * scalar},
+            nullptr);
+    }
+
     float *coordinates;
 
     Triangle *triangle;
@@ -116,11 +130,52 @@ class KDTree {
     }
 
     Point *intersect_ray_recurse(Ray ray, Node *node) {
-        // Intersect ray with the point's splitting plane
-        // If there is an intersection: Recurse to both children (but the nearer one first)
-        // Otherwise: Recurse only to the nearer one
+        // Exit condition: There was no collision
+        if (node == nullptr) { return nullptr; }
 
-        return node->point; // TODO
+        // Is the left or right child node closer to this point?
+        Node *near = ray.origin[node->axis] > node->point->coordinates[node->axis] ? node->right
+                                                                                   : node->left;
+        Node *far = near == node->right ? node->left : node->right;
+
+        std::cout << "Checking " << node->point->coordinates[0] << ", "
+                  << node->point->coordinates[1] << ", " << node->point->coordinates[2] << ", "
+                  << std::endl;
+
+        // Intersect ray with the point's splitting plane
+
+        // Are they parallel? If so, recurse only to the nearer side
+        if (ray.direction[node->axis] == 0.0) {
+            return intersect_ray_recurse(ray, near);
+        } else {
+            // They are not parallel, so check where the intersection occurs
+            float t = (node->point->coordinates[node->axis] - ray.origin[node->axis]) /
+                      ray.direction[node->axis];
+
+            if (t >= 0.0) {
+                // t is positive, so we need to recurse to both children if the nearer one does not
+                // result in a collision
+                Point *first_attempt = intersect_ray_recurse(ray, near);
+
+                if (first_attempt != nullptr) {
+                    return first_attempt;
+                } else {
+                    // The first attempt did not work, so recurse to the other side too.
+                    // For this, calculate a new ray origin ...
+                    float new_origin[3]{ray.origin[0] + t * ray.direction[0],
+                                        ray.origin[1] + t * ray.direction[1],
+                                        ray.origin[2] + t * ray.direction[2]};
+
+                    // ... and continue towards that direction, but with the new origin (we can
+                    // leave behind what we already checked)
+                    return intersect_ray_recurse(Ray(new_origin, ray.direction), far);
+                }
+            } else {
+                // We only have to check the nearer one, as the other side can't be reached by the
+                // ray
+                return intersect_ray_recurse(ray, near);
+            }
+        }
     }
 
     void to_string_recurse(std::string &str, Node *node, int depth) {

main.cpp

@@ -8,11 +8,14 @@ int main() {
     std::vector<Point *> points{new Point(new float[3]{0.0, 0.0, 0.0}, nullptr),
                                 new Point(new float[3]{0.0, 1.0, 0.0}, nullptr),
                                 new Point(new float[3]{0.0, 2.0, 3.0}, nullptr),
-                                new Point(new float[3]{1.0, 0.0, 4.0}, nullptr)};
+                                new Point(new float[3]{1.0, 0.0, 4.0}, nullptr),
+                                new Point(new float[3]{1.0, -1.0, 8.0}, nullptr)};
 
     KDTree tree = KDTree(points);
 
     std::cout << tree.to_string();
 
+    tree.intersect_ray(Ray(new float[3]{0.0, 0.0, 5.0}, new float[3]{0.0, 0.0, -1.0}));
+
     return 0;
 }