diff --git a/geometry.h b/geometry.h
index 35ac533..980fb30 100644
--- a/geometry.h
+++ b/geometry.h
@@ -1,4 +1,8 @@
+#include <vector>
+
 // Forward declarations
+struct Triangle;
+
 struct Vector {
     Vector(float coordinates[3]) : c(coordinates) {}
     Vector(float x, float y, float z) : c(new float[3]{x, y, z}) {}
@@ -20,32 +24,35 @@ struct Vector {
     }
 
     Vector cross(const Vector &other) {
-        return Vector(c[2] * other[3] - c[3] - other[2], c[3] * other[1] - c[1] * other[3],
-                      c[1] * other[2] - c[2] * other[1]);
+        return Vector(c[1] * other[2] - c[2] - other[1], c[2] * other[0] - c[0] * other[2],
+                      c[0] * other[1] - c[1] * other[0]);
     }
 
-    float dot(const Vector &other) { return c[1] * other[1] + c[2] * other[2] + c[3] * other[3]; }
+    float dot(const Vector &other) { return c[0] * other[0] + c[1] * other[1] + c[2] * other[2]; }
 
     float *c;
 };
 
+struct Point {
+    Point(Vector pos, Triangle *triangle) : pos(pos), triangle(triangle) {}
+
+    Vector pos;
+
+    Triangle *triangle;
+};
+
 struct Triangle {
     Triangle(Vector p1, Vector p2, Vector p3) : p1(p1), p2(p2), p3(p3) {}
 
+    std::vector<Point *> create_point_objects() {
+        return std::vector<Point *>{new Point(p1, this), new Point(p2, this), new Point(p3, this)};
+    }
+
     Vector p1;
     Vector p2;
     Vector p3;
 };
 
-struct Point {
-    Point(float coordinates[3], Triangle *triangle)
-        : pos(Vector(coordinates)), triangle(triangle) {}
-
-    Vector pos;
-
-    Triangle *triangle;
-};
-
 struct Node {
     Node(int axis, Point *point, Node *left, Node *right)
         : axis(axis), point(point), left(left), right(right) {}
diff --git a/kdtree.h b/kdtree.h
index 653a438..3ba49a5 100644
--- a/kdtree.h
+++ b/kdtree.h
@@ -11,7 +11,7 @@ class KDTree {
 
     ~KDTree() = default; // TODO: Delete all allocated Nodes
 
-    Point *intersect_ray(Ray ray) { return intersect_ray_recurse(ray, root, 1000.0); }
+    Triangle *intersect_ray(Ray ray) { return intersect_ray_recurse(ray, root, 1000.0); }
 
     std::string to_string() {
         std::string str = "";
@@ -75,7 +75,7 @@ class KDTree {
                         build(right_of_median, depth + 1));
     }
 
-    Point *intersect_ray_recurse(Ray ray, Node *node, float max_distance) {
+    Triangle *intersect_ray_recurse(Ray ray, Node *node, float max_distance) {
         // Exit condition: There was no collision
         if (node == nullptr) { return nullptr; }
 
@@ -85,7 +85,7 @@ class KDTree {
         Node *far = near == node->right ? node->left : node->right;
 
         std::cout << "Checking " << node->point->pos[0] << ", " << node->point->pos[1] << ", "
-                  << node->point->pos[2] << ", " << std::endl;
+                  << node->point->pos[2] << std::endl;
 
         // Check for collisions in this order (stopping if an intersection is found):
         // 1. In the nearer section
@@ -98,16 +98,16 @@ class KDTree {
                       ? (node->point->pos[node->axis] - ray.origin[node->axis]) /
                             ray.direction[node->axis]
                       : max_distance;
-        Point *near_result = intersect_ray_recurse(ray, near, t);
+        Triangle *near_result = intersect_ray_recurse(ray, near, t);
 
         // If the nearer segment had a collision, we're done! We're only interested in the closest
         // collision.
         if (near_result != nullptr) { return near_result; }
 
         // No collision in the nearer side, so check for a collision directly here
-        if (node->point->triangle && ray.intersects_triangle(node->point->triangle)) {
+        if (ray.intersects_triangle(node->point->triangle)) {
             // We do have a collision here, so we're done and can return this point!
-            return node->point;
+            return node->point->triangle;
         }
 
         // No collision here either. Does it make sense to also check the far node?
@@ -129,7 +129,7 @@ class KDTree {
 
         Point *point = node->point;
 
-        str += std::string(depth, '-') + std::to_string(point->pos[0]) + ", " +
+        str += std::string(depth * 2, ' ') + std::to_string(point->pos[0]) + ", " +
                std::to_string(point->pos[1]) + ", " + std::to_string(point->pos[2]) +
                " with axis " + std::to_string(node->axis) + "\n";
 
diff --git a/main.cpp b/main.cpp
index 794ebbd..a7a8b80 100644
--- a/main.cpp
+++ b/main.cpp
@@ -2,20 +2,24 @@
 #include <iostream>
 
 int main() {
-    std::cout << "Hello World!" << std::endl;
+    // Testing triangles
+    Triangle t1 = Triangle(Vector(0.0, 0.0, 0.0), Vector(2.0, 0.0, 0.0), Vector(0.0, 2.0, 0.0));
+    Triangle t2 = Triangle(Vector(0.5, 0.0, 0.0), Vector(1.0, 1.0, 0.0), Vector(1.0, 1.0, 1.0));
 
-    // Test points
-    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, -1.0, 8.0}, nullptr)};
+    // Create list of all points from triangles
+    std::vector<Point *> points = t1.create_point_objects();
+    std::vector<Point *> other_points = t2.create_point_objects();
+    points.insert(points.end(), other_points.begin(), other_points.end());
 
+    // Create and print the KDTree resulting from these points
     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}));
+    // Intersection check
+    Triangle *intersection =
+        tree.intersect_ray(Ray(new float[3]{0.5, 0.5, -1.0}, new float[3]{0.0, 0.1, 1.0}));
+
+    if (intersection != nullptr) { std::cout << "Hit!" << std::endl; }
 
     return 0;
 }