Add kdtree and foundation of CollisionSystem

ECS/Components/Mesh.h

@@ -11,7 +11,7 @@
 
 struct Mesh {
     explicit Mesh(const std::vector<float> &_vertices, const std::vector<unsigned int> &_indices)
-        : vertex_count(_indices.size()) {
+        : vertex_count(_indices.size()), vertices(_vertices), indices(_indices) {
         // Copy the vertices into a local classic float array. Nothing was displayed without this,
         // maybe
         //  due to weird hidden type incompatibility or out of scope issues?
@@ -68,11 +68,14 @@ struct Mesh {
         glDrawElements(GL_TRIANGLES, vertex_count, GL_UNSIGNED_INT, 0);
     }
 
+    std::vector<float> vertices;
+    std::vector<unsigned int> indices;
+    unsigned int vertex_count;
+
   private:
     unsigned int EBO;
     unsigned int VBO;
     unsigned int VAO;
-    unsigned int vertex_count;
 };
 
 #endif // ECSGAME_MESH_H

ECS/Systems/CollisionSystem.h

@@ -0,0 +1,79 @@
+#pragma once
+
+#include "../../Util/kdtree.h"
+#include "../Components/LODObjMesh.h"
+#include "../Components/Mesh.h"
+#include "../Components/ObjMesh.h"
+#include "../Components/Transform.h"
+#include "../ECS.h"
+
+using namespace ECS;
+
+class CollisionSystem : public EntitySystem {
+  public:
+    // Initialize the kdtree
+    void build() {
+        std::vector<Triangle *> triangles;
+        std::vector<Point *> points;
+
+        // ObjMesh
+        myWorld->each<ObjMesh, Transform>(
+            [&](Entity *ent, ComponentHandle<ObjMesh> mesh, ComponentHandle<Transform> transform) {
+                std::vector<unsigned int> indices = mesh->indices;
+                std::vector<float> vertices = mesh->vertices;
+
+                for (int i = 0; i < mesh->vertex_count; i += 3) {
+                    float v0p0 = vertices[indices[i + 0] * 14 + 0];
+                    float v0p1 = vertices[indices[i + 0] * 14 + 1];
+                    float v0p2 = vertices[indices[i + 0] * 14 + 2];
+
+                    float v1p0 = vertices[indices[i + 1] * 14 + 0];
+                    float v1p1 = vertices[indices[i + 1] * 14 + 1];
+                    float v1p2 = vertices[indices[i + 1] * 14 + 2];
+
+                    float v2p0 = vertices[indices[i + 2] * 14 + 0];
+                    float v2p1 = vertices[indices[i + 2] * 14 + 1];
+                    float v2p2 = vertices[indices[i + 2] * 14 + 2];
+
+                    glm::vec4 v1glm(v0p0, v0p1, v0p2, 1.0);
+                    glm::vec4 v2glm(v1p0, v1p1, v1p2, 1.0);
+                    glm::vec4 v3glm(v2p0, v2p1, v2p2, 1.0);
+
+                    // Transform to World Position -- these are local coordinates with
+                    // individual mesh origins
+                    v1glm = transform->matrix * v1glm;
+                    v2glm = transform->matrix * v2glm;
+                    v3glm = transform->matrix * v3glm;
+
+                    Vector v1(v1glm.x, v1glm.y, v1glm.z);
+                    Vector v2(v2glm.x, v2glm.y, v2glm.z);
+                    Vector v3(v3glm.x, v3glm.y, v3glm.z);
+
+                    Triangle *triangle = new Triangle(v1, v2, v3);
+                    triangles.emplace_back(triangle);
+
+                    points.emplace_back(new Point(v1, triangle));
+                    points.emplace_back(new Point(v2, triangle));
+                    points.emplace_back(new Point(v3, triangle));
+                }
+            });
+
+        // LODObjMesh
+        myWorld->each<LODObjMesh, Transform>([&](Entity *ent, ComponentHandle<LODObjMesh> lodMesh,
+                                                 ComponentHandle<Transform> transform) {
+            // TODO
+        });
+
+        std::cout << "Start building kdtree with " << points.size() << " points" << std::endl;
+        kdtree = new KDTree(points);
+        std::cout << "Done" << std::endl;
+
+        std::cout << kdtree->to_string() << std::endl;
+    }
+
+    void configure(World *pWorld) override { myWorld = pWorld; }
+
+    World *myWorld;
+
+    KDTree *kdtree;
+};

Util/geometry.h

@@ -0,0 +1,112 @@
+#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}) {}
+
+    // Avoid having to write vector.c[index], instead allow vector[index]
+    float operator[](int i) const { return c[i]; }
+    float &operator[](int i) { return c[i]; }
+
+    Vector operator+(const Vector &other) const {
+        return Vector(c[0] + other.c[0], c[1] + other.c[1], c[2] + other.c[2]);
+    }
+
+    Vector operator-(const Vector &other) const {
+        return Vector(c[0] - other.c[0], c[1] - other.c[1], c[2] - other.c[2]);
+    }
+
+    Vector operator*(float scalar) const {
+        return Vector(c[0] * scalar, c[1] * scalar, c[2] * scalar);
+    }
+
+    Vector cross(const Vector &other) {
+        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[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 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 Ray {
+    Ray(Vector origin, Vector direction) : origin(origin), direction(direction) {}
+
+    Vector origin;
+
+    Vector direction;
+
+    bool intersects_triangle(Triangle *triangle) {
+        // Ray-triangle-intersection with the Möller–Trumbore algorithm
+        // https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+        const float EPSILON = 0.0000001;
+
+        Vector p1 = triangle->p1;
+        Vector p2 = triangle->p2;
+        Vector p3 = triangle->p3;
+
+        Vector edge1 = p2 - p1;
+        Vector edge2 = p3 - p1;
+
+        Vector h = direction.cross(edge2);
+        float a = edge1.dot(h);
+
+        if (a > -EPSILON && a < EPSILON) return false; // This ray is parallel to this triangle.
+
+        float f = 1.0 / a;
+        Vector s = origin - p1;
+        float u = f * s.dot(h);
+
+        if (u < 0.0 || u > 1.0) return false;
+
+        Vector q = s.cross(edge1);
+        float v = f * direction.dot(q);
+        if (v < 0.0 || u + v > 1.0) return false;
+
+        // At this stage we can compute t to find out where the intersection point is on the
+        // line.
+        float t = f * edge2.dot(q);
+        if (t > EPSILON) {
+            return true;
+        } else {
+            // This means that there is a line intersection but not a ray intersection.
+            return false;
+        }
+    }
+};

Util/kdtree.h

@@ -0,0 +1,139 @@
+#include "geometry.h"
+#include <algorithm>
+#include <glm/glm.hpp>
+#include <iostream>
+#include <string>
+#include <vector>
+
+class KDTree {
+  public:
+    KDTree(std::vector<Point *> points) { root = build(points, 0); }
+
+    ~KDTree() = default; // TODO: Delete all allocated Nodes
+
+    Triangle *intersect_ray(Ray ray) { return intersect_ray_recurse(ray, root, 1000.0); }
+
+    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->pos[axis] < p2->pos[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::min_element(points.begin(), points.end(), comparator);
+            Point *max = *std::max_element(points.begin(), points.end(), comparator);
+
+            float extent = max->pos[it_axis] - min->pos[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 + 1, 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));
+    }
+
+    Triangle *intersect_ray_recurse(Ray ray, Node *node, float max_distance) {
+        // Exit condition: There was no collision
+        if (node == nullptr) { return nullptr; }
+
+        // Is the left or right child node closer to this point?
+        Node *near =
+            ray.origin[node->axis] > node->point->pos[node->axis] ? node->right : node->left;
+        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;
+
+        // Check for collisions in this order (stopping if an intersection is found):
+        // 1. In the nearer section
+        // 2. With the point in this current node
+        // 3. In the further section
+
+        // If the axes are not parallel, our max_distance decreases, since we've already covered
+        // some area. `t` represents the distance from this node to the splitting plane.
+        float t = ray.direction[node->axis] != 0.0
+                      ? (node->point->pos[node->axis] - ray.origin[node->axis]) /
+                            ray.direction[node->axis]
+                      : max_distance;
+        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 (ray.intersects_triangle(node->point->triangle)) {
+            // We do have a collision here, so we're done and can return this point!
+            return node->point->triangle;
+        }
+
+        // No collision here either. Does it make sense to also check the far node?
+        // Only if the axes are not parallel and if that area is not behind us
+        if (ray.direction[node->axis] != 0.0 && t >= 0.0) {
+            // It does make sense to check the far node.
+            // For this, calculate a new ray origin and continue towards that direction, but with
+            // the new origin (we can leave behind what we already checked)
+            return intersect_ray_recurse(Ray(ray.origin + ray.direction * t, ray.direction), far,
+                                         max_distance - t);
+        }
+
+        // If nothing worked, return a nullptr
+        return nullptr;
+    }
+
+    void to_string_recurse(std::string &str, Node *node, int depth) {
+        if (node == nullptr) { return; }
+
+        Point *point = node->point;
+
+        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";
+
+        to_string_recurse(str, node->left, depth + 1);
+        to_string_recurse(str, node->right, depth + 1);
+    }
+};

main.cpp

@@ -10,6 +10,7 @@
 #include "ECS/ECS.h"
 #include "ECS/Events/InputEvent.h"
 #include "ECS/Events/MouseMoveEvent.h"
+#include "ECS/Systems/CollisionSystem.h"
 #include "ECS/Systems/GravitySystem.h"
 #include "ECS/Systems/InteractivePathSystem.h"
 #include "ECS/Systems/KeyboardMovementSystem.h"
@@ -89,6 +90,9 @@ int main(int argc, char **argv) {
     world->registerSystem(new SineAnimationSystem());
     world->registerSystem(new InteractivePathSystem());
 
+    CollisionSystem *collision_system = new CollisionSystem();
+    world->registerSystem(collision_system);
+
     RenderSystem *renderSystem = new RenderSystem();
     world->registerSystem(renderSystem);
 
@@ -192,6 +196,9 @@ int main(int argc, char **argv) {
     Entity *sun = world->create();
     sun->assign<DirectionalLight>(glm::normalize(glm::vec3(1.0, 1.0, 1.0)));
 
+    // We're done loading geometry -> build the collision structure
+    collision_system->build();
+
     Shader defaultShader("Shaders/default-vertex.vs", "Shaders/default-fragment.fs");
     Shader shadowShader("Shaders/shadow-vertex.vs", "Shaders/shadow-fragment.fs");
     Shader debugShader("Shaders/debug-vertex.vs", "Shaders/debug-fragment.fs");