From 87b54a1b41c9e3fd4411f6e84cd523bc1474a9f2 Mon Sep 17 00:00:00 2001 From: karl Date: Sat, 2 Jan 2021 16:00:09 +0100 Subject: [PATCH] Add kdtree and foundation of CollisionSystem --- ECS/Components/Mesh.h | 7 +- ECS/Systems/CollisionSystem.h | 79 +++++++++++++++++++ Util/geometry.h | 112 +++++++++++++++++++++++++++ Util/kdtree.h | 139 ++++++++++++++++++++++++++++++++++ main.cpp | 7 ++ 5 files changed, 342 insertions(+), 2 deletions(-) create mode 100644 ECS/Systems/CollisionSystem.h create mode 100644 Util/geometry.h create mode 100644 Util/kdtree.h diff --git a/ECS/Components/Mesh.h b/ECS/Components/Mesh.h index c840b9b..c453156 100644 --- a/ECS/Components/Mesh.h +++ b/ECS/Components/Mesh.h @@ -11,7 +11,7 @@ struct Mesh { explicit Mesh(const std::vector &_vertices, const std::vector &_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 vertices; + std::vector indices; + unsigned int vertex_count; + private: unsigned int EBO; unsigned int VBO; unsigned int VAO; - unsigned int vertex_count; }; #endif // ECSGAME_MESH_H diff --git a/ECS/Systems/CollisionSystem.h b/ECS/Systems/CollisionSystem.h new file mode 100644 index 0000000..d90f903 --- /dev/null +++ b/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 triangles; + std::vector points; + + // ObjMesh + myWorld->each( + [&](Entity *ent, ComponentHandle mesh, ComponentHandle transform) { + std::vector indices = mesh->indices; + std::vector 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([&](Entity *ent, ComponentHandle lodMesh, + ComponentHandle 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; +}; diff --git a/Util/geometry.h b/Util/geometry.h new file mode 100644 index 0000000..980fb30 --- /dev/null +++ b/Util/geometry.h @@ -0,0 +1,112 @@ +#include + +// 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 create_point_objects() { + return std::vector{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; + } + } +}; \ No newline at end of file diff --git a/Util/kdtree.h b/Util/kdtree.h new file mode 100644 index 0000000..3ba49a5 --- /dev/null +++ b/Util/kdtree.h @@ -0,0 +1,139 @@ +#include "geometry.h" +#include +#include +#include +#include +#include + +class KDTree { + public: + KDTree(std::vector 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 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 left_of_median(points.begin(), points.begin() + middle); + std::vector 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); + } +}; diff --git a/main.cpp b/main.cpp index db2be66..17d161d 100644 --- a/main.cpp +++ b/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(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");