add engine version

1 files changed, 370 insertions, 0 deletions

diff --git a/engine-ocean/Game/Systems/CollisionSystems/ellipsoidtrianglecollisionsystem.cpp b/engine-ocean/Game/Systems/CollisionSystems/ellipsoidtrianglecollisionsystem.cpp
new file mode 100644
index 0000000..52b8916
--- /dev/null
+++ b/engine-ocean/Game/Systems/CollisionSystems/ellipsoidtrianglecollisionsystem.cpp
@@ -0,0 +1,370 @@
+#include "ellipsoidtrianglecollisionsystem.h"
+#include "Game/Components/CollisionComponents/BoundingTriangle.h"
+#include "Game/Components/CollisionComponents/CollisionComponent.h"
+#include "Game/Components/CollisionComponents/boundingellipsoid.h"
+#include "Game/Components/DrawComponent.h"
+#include "Game/Components/TransformComponent.h"
+#include "Game/GameObjects/GameObject.h"
+#include <memory>
+
+EllipsoidTriangleCollisionSystem::EllipsoidTriangleCollisionSystem(std::map<std::string, std::shared_ptr<GameObject>>& rigid_gameobjects) :
+    m_rigid_gameobjects(rigid_gameobjects),
+    m_bvh_system(std::make_unique<BVHTree>(rigid_gameobjects))
+{
+}
+
+
+Triangle EllipsoidTriangleCollisionSystem::convertTriangleToSphereSpace(const glm::vec3 &orig_R, const Triangle &orig_triangle){
+    Triangle transformed_triangle;
+
+    // divide vertex pos by Rs
+    transformed_triangle.vertexA = orig_triangle.vertexA / orig_R;
+    transformed_triangle.vertexB = orig_triangle.vertexB / orig_R;
+    transformed_triangle.vertexC = orig_triangle.vertexC / orig_R;
+
+    transformed_triangle.edge1 = transformed_triangle.vertexB - transformed_triangle.vertexA;
+    transformed_triangle.edge2 = transformed_triangle.vertexC - transformed_triangle.vertexA;
+
+    // does the normal stay the same across transformations?
+    transformed_triangle.normal = glm::normalize(glm::cross(transformed_triangle.edge1, transformed_triangle.edge2));
+
+    return transformed_triangle;
+}
+
+
+glm::vec3 EllipsoidTriangleCollisionSystem::getIntersectionPt(const glm::vec3 &ray_p, const glm::vec3 &ray_d, const float t){
+    return ray_p + t*ray_d;
+}
+
+float EllipsoidTriangleCollisionSystem::solveQuadratic(float a, float b, float c){
+    float discriminant = pow(b,2) - 4*a*c;
+
+    // check for imaginary numbers
+    if (discriminant >= 0){
+        float pos_quad = (-b + sqrt(discriminant)) / (2*a);
+        float neg_quad = (-b - sqrt(discriminant)) / (2*a);
+
+        // if the mininum is a neg number, then return the larger of the two
+        if (fmin(pos_quad, neg_quad) < ZERO){
+            return fmax(pos_quad, neg_quad);
+        } else {
+            return fmin(pos_quad, neg_quad);
+        }
+    }
+    return -1;
+}
+
+
+float EllipsoidTriangleCollisionSystem::raycastSphere(const glm::vec3 &ray_p, const glm::vec3 &ray_d, const glm::vec3 posA, const glm::vec3 posB){
+    // shift ray and ellipsoid to center first in order to use regular implicit sphere equation
+    glm::vec3 p = ray_p - posA;
+    glm::vec3 d = ray_d - posA;
+
+    float a = (d.x*d.x) + (d.y*d.y) + (d.z*d.z);
+    float b = (2.f*p.x*d.x) + (2.f*p.y*d.y) + (2.f*p.z*d.z);
+    float c = (p.x*p.x) + (p.y*p.y) + (p.z*p.z) - 1.f;
+
+    return solveQuadratic(a,b,c);
+}
+
+float EllipsoidTriangleCollisionSystem::raycastTriangle(const glm::vec3 &ray_p, const glm::vec3 &ray_d, const Triangle &triangle){
+    float t = (glm::dot((triangle.vertexA - ray_p), triangle.normal))/(glm::dot(ray_d, triangle.normal));
+    return t;
+}
+
+glm::vec3 EllipsoidTriangleCollisionSystem::getTriangleEdge(const glm::vec3 v1, const glm::vec3 v2){
+    return v2 - v1;
+}
+
+bool EllipsoidTriangleCollisionSystem::isSameDirection(const glm::vec3 edge1, const glm::vec3 edge2, const glm::vec3 n){
+    if (glm::dot(glm::cross(edge1, edge2), n) > 0){
+        return true;
+    }
+    return false;
+}
+
+void EllipsoidTriangleCollisionSystem::makeCollisionData(const float t, const glm::vec3 triangle_n,
+                                                         const glm::vec3 intersection_pos,
+                                                         const glm::vec3 point_of_contact,
+                                                         const bool isVertex){
+    CollisionData data;
+    data.t = t;
+    data.triangle_n = triangle_n;
+    data.intersection_pos = intersection_pos;
+    data.point_of_contact = point_of_contact;
+    data.isVertex = isVertex;
+    m_collisionData.push_back(data);
+}
+
+
+bool EllipsoidTriangleCollisionSystem::detectSphere_with_Interior(const Triangle &triangle, const glm::vec3 posA, const glm::vec3 posB){
+    // calculate point on sphere closest to the plane and cast a ray
+    glm::vec3 ray_p = posA-triangle.normal; // closest point on unit sphere
+    glm::vec3 ray_d = (posB-posA);
+
+    glm::vec3 n = triangle.normal;
+    glm::vec3 v = triangle.vertexA;
+    float plane_const = -1 * (n.x*v.x + n.y*v.y + n.z*v.z);
+
+    // pdf equation
+    float t = (1-(glm::dot(n, posA) + plane_const)) / (glm::dot(n, (posB-posA)));
+    if (t < ZERO || t > ONE){
+        return false;
+    }
+
+    // the point P on the sphere closest to plane surface
+    glm::vec3 P_edge = getIntersectionPt(ray_p, ray_d, t);
+
+
+    // determine if point is on triangle plane
+        // determine if point is also within triangle bounds
+        glm::vec3 AB = getTriangleEdge(triangle.vertexA, triangle.vertexB);
+        glm::vec3 BC = getTriangleEdge(triangle.vertexB, triangle.vertexC);
+        glm::vec3 CA = getTriangleEdge(triangle.vertexC, triangle.vertexA);
+
+        glm::vec3 AP = getTriangleEdge(triangle.vertexA, P_edge);
+        glm::vec3 BP = getTriangleEdge(triangle.vertexB, P_edge);
+        glm::vec3 CP = getTriangleEdge(triangle.vertexC, P_edge);
+
+        if (isSameDirection(AB, AP, triangle.normal) &&
+                isSameDirection(BC, BP, triangle.normal) &&
+                isSameDirection(CA, CP, triangle.normal)){
+
+            // the actual center of sphere
+            glm::vec3 P_centered = getIntersectionPt(posA, posB-posA, t);
+            makeCollisionData(t, triangle.normal, P_centered, P_edge, false);
+            return true;
+        }
+    return false;
+}
+
+// do this for each vertex pair AB, BC, CA
+bool EllipsoidTriangleCollisionSystem::detectSphere_with_Edge(const glm::vec3 &v1, const glm::vec3 &v2,
+                                                              const glm::vec3 &triangle_normal,
+                                                              const glm::vec3 &posA, const glm::vec3 &posB){
+    glm::vec3 C = v1;
+    glm::vec D = v2;
+
+    float a = pow(glm::length(glm::cross(posB-posA, D-C)),2);
+    float b = glm::dot(2.f*glm::cross(posB-posA, D-C), glm::cross(posA-C, D-C));
+    float c = pow(glm::length(glm::cross(posA-C, D-C)),2) - pow(glm::length(D-C),2);
+
+    float t = solveQuadratic(a,b,c);
+
+    // discard a negative t --> indicates no collision with edge
+    if (t < ZERO || t > ONE){
+        return false;
+    }
+
+    glm::vec3 P = getIntersectionPt(posA, posB-posA, t);
+
+    glm::vec3 AP = P-C;
+    glm::vec3 AB = D-C;
+    glm::vec3 normal = AP - (glm::dot(AB,AP)/glm::dot(AB,AB))*AB;
+
+    // determine if intersection pt is between C and D
+    if ( (0 < glm::dot(P-C,D-C)) &&  (glm::dot(P-C,D-C) < pow(glm::length(D-C),2)) ){
+        makeCollisionData(t, normal, P, P, false);
+        return true;
+    }
+
+    return false;
+}
+
+// needs to be done with each vertex
+bool EllipsoidTriangleCollisionSystem::detectSphere_with_Vertex(const glm::vec3 &v, const glm::vec3 triangle_normal, const glm::vec3 &posA, const glm::vec3 &posB){
+    glm::vec3 ray_p = v;
+    //glm::vec3 ray_d = v-(posB-posA);
+
+    // convert to origin space:
+    glm::vec3 vel = posB-posA;
+    glm::vec3 v_origined = v - posA;
+
+    float a = glm::dot(vel,vel);
+    float b = 2*(glm::dot(vel, -v_origined));
+    float c = pow(glm::length(v_origined),2) - 1.f;
+
+    float t = solveQuadratic(a,b,c);
+    if (t < ZERO || t > ONE){
+        return false;
+    }
+
+    // where the center is when the sphere hits vertex
+    glm::vec3 center_pt_origined = t*vel;
+    // convert back to non-origin space
+    glm::vec3 center_pt = center_pt_origined + posA;
+    glm::vec3 normal = center_pt - v;
+    makeCollisionData(t, normal, center_pt, v, true);
+     return true;
+}
+
+CollisionData EllipsoidTriangleCollisionSystem::getNearestCollision(const std::vector<CollisionData> &collisionData, const Triangle &sphere_tri){
+    CollisionData nearestCollision;
+    nearestCollision.t = INFINITY;
+
+    for (const CollisionData &data : collisionData){
+        if (data.t < nearestCollision.t){
+            // set nearest collision to datapoint
+            nearestCollision = data;
+        }
+    }
+    return nearestCollision;
+}
+
+// SPHERE SPACE
+CollisionData EllipsoidTriangleCollisionSystem::detectTriangleCollision(const Triangle &sphere_tri, const glm::vec3 &posA, const glm::vec3 &posB){
+
+    m_collisionData.clear();
+    // if interior is true, no need to check rest. get the only collision data
+    if (detectSphere_with_Interior(sphere_tri, posA, posB)){
+        return m_collisionData[0];
+    }
+
+    // otherwise check rest:
+
+    // detect edge with all 3 edges
+    // | operator executes regardless of if left hand side is true or not
+    if (
+    detectSphere_with_Edge(sphere_tri.vertexA, sphere_tri.vertexB, sphere_tri.normal, posA, posB) |
+    detectSphere_with_Edge(sphere_tri.vertexB, sphere_tri.vertexC, sphere_tri.normal, posA, posB) |
+    detectSphere_with_Edge(sphere_tri.vertexC, sphere_tri.vertexA, sphere_tri.normal, posA, posB) ){
+        return getNearestCollision(m_collisionData, sphere_tri);
+    }
+
+    // detect vertex with all 3 vertices
+    detectSphere_with_Vertex(sphere_tri.vertexA, sphere_tri.normal, posA, posB);
+    detectSphere_with_Vertex(sphere_tri.vertexB, sphere_tri.normal, posA, posB);
+    detectSphere_with_Vertex(sphere_tri.vertexC, sphere_tri.normal, posA, posB);
+
+    return getNearestCollision(m_collisionData, sphere_tri);
+}
+
+CollisionData EllipsoidTriangleCollisionSystem::revertDataToOriginalSpace(const CollisionData &sphere_datum, const glm::vec3 &ellip_R, const glm::vec3 orig_triangle_n){
+    CollisionData reverted_datum = sphere_datum;
+    reverted_datum.intersection_pos = reverted_datum.intersection_pos*ellip_R;
+    reverted_datum.point_of_contact = reverted_datum.point_of_contact*ellip_R;
+
+    return reverted_datum;
+}
+
+void EllipsoidTriangleCollisionSystem::setEllipsoidDimensions(const glm::vec3 &dimensions){
+    m_ellipsoid_dimensions = dimensions;
+}
+
+CollisionData EllipsoidTriangleCollisionSystem::collideWithWorld(const glm::vec3 &posA, const glm::vec3 &posB){
+    // makes ellipsoids
+    Ellipsoid ellipsoidA;
+    ellipsoidA.R = m_ellipsoid_dimensions;
+    ellipsoidA.center_pos = posA;
+
+
+    Ellipsoid ellipsoidB;
+    ellipsoidB.R = m_ellipsoid_dimensions;
+    ellipsoidB.center_pos = posB;
+
+    // convert ellipsoid centers to sphere space
+    glm::vec3 sphere_posA = ellipsoidA.center_pos/m_ellipsoid_dimensions;
+    glm::vec3 sphere_posB = ellipsoidB.center_pos/m_ellipsoid_dimensions;
+
+    CollisionData nearestCollision;
+    nearestCollision.t = INFINITY;
+
+
+    std::vector<Triangle> candidates = m_bvh_system->getBVHDetectedCollisions(posA, posB, ellipsoidA.R);
+   // std::cout << "candidate size: " << candidates.size() << std::endl;
+
+
+//    for (auto &rigid_go : candidates){
+//        //std::cout << "candidate shape name: " << rigid_go->getComponent<DrawComponent>()->getShapeName() << std::endl;
+//        if (rigid_go->hasComponent<CollisionComponent>()){
+//            if (rigid_go->getComponent<CollisionComponent>()->hasCollisionShape<BoundingTriangle>()){
+//                // for every triangle in game obj, transform to sphere space and collide with ellipsoid (aka sphere)
+                for (int i=0; i< candidates.size(); i++){
+                    Triangle triangle = candidates[i];
+//                    std::cout<< "triangle i: " << i << ", v1: (" << triangle.vertexA.x << ", " << triangle.vertexA.y << ", " << triangle.vertexA.z << "), v2: ("
+//                                                                 << triangle.vertexB.x << ", " << triangle.vertexB.y << ", " << triangle.vertexB.z << "), v3: ("
+//                                                                 << triangle.vertexC.x << ", " << triangle.vertexC.y << ", " << triangle.vertexC.z << ")" << std::endl;
+
+                    // transform triangle to sphere space and detect collision data (in sphere space) for this specific triangle
+                    Triangle sphere_triangle = convertTriangleToSphereSpace(m_ellipsoid_dimensions, triangle);
+                    CollisionData sphere_collisionDatum = detectTriangleCollision(sphere_triangle, sphere_posA, sphere_posB);
+                    CollisionData ellip_collisionDatum = revertDataToOriginalSpace(sphere_collisionDatum, m_ellipsoid_dimensions, triangle.normal);
+
+                    // keep track of nearest Collision
+                    if (ellip_collisionDatum.t < nearestCollision.t && ellip_collisionDatum.t <= ONE && ellip_collisionDatum.t >= ZERO){
+                        nearestCollision = ellip_collisionDatum;
+                    }
+                    // repeat for the other triangles
+                }
+            //}
+        //}
+        // repeat for all gameobjects
+    //}
+
+    return nearestCollision;
+}
+
+glm::vec3 EllipsoidTriangleCollisionSystem::doNudge(glm::vec3 curr_pos, CollisionData collision){
+    glm::vec3 nudge = m_scale_normals_mt*collision.triangle_n;
+
+    glm::vec3 pos_nudged =  collision.intersection_pos + nudge * .01f;
+    int MAX_NUDGES = 3;
+    for (int i=0; i<MAX_NUDGES; i++){
+        CollisionData nudge_collision = collideWithWorld(curr_pos, pos_nudged);
+        if (nudge_collision.t == INFINITY){
+            curr_pos = pos_nudged;
+            break;
+        } else {
+            if (glm::length((m_scale_normals_mt*nudge_collision.triangle_n) - nudge) < .0001 || glm::length((m_scale_normals_mt*nudge_collision.triangle_n) + nudge) < .0001){
+                nudge = -m_scale_normals_mt*nudge_collision.triangle_n;
+            } else {
+                nudge = m_scale_normals_mt*nudge_collision.triangle_n;
+            }
+            pos_nudged = nudge_collision.intersection_pos + nudge * .01f;
+        }
+    }
+    return curr_pos;
+
+}
+
+glm::mat3 EllipsoidTriangleCollisionSystem::makeConversionMT(glm::vec3 initial_center_pos, glm::vec3 dimensions){
+    m_conversion_mt = std::make_shared<ModelTransform>();
+    m_conversion_mt->translate(-initial_center_pos);
+    m_conversion_mt->scale(dimensions);
+    m_scale_normals_mt = glm::transpose((glm::inverse(glm::mat3(m_conversion_mt->getModelMatrix()))));
+    m_scale_normals_mt = glm::mat4(1.f);
+    return m_scale_normals_mt;
+}
+
+// in sphere space
+std::pair<std::vector<CollisionData>, glm::vec3> EllipsoidTriangleCollisionSystem::mtvSlide(glm::vec3 initial_pos, glm::vec3 final_pos, glm::vec3 dimensions){
+    setEllipsoidDimensions(dimensions);
+    glm::mat3 normals_scale = makeConversionMT(initial_pos, dimensions);
+
+    std::vector<CollisionData> collisions;
+    glm::vec3 curr_pos = initial_pos;
+    glm::vec3 next_pos = final_pos;
+    int MAX_TRANSLATIONS = 3;
+    for (int i=0; i<MAX_TRANSLATIONS; i++){
+        CollisionData c = collideWithWorld(initial_pos, next_pos);
+
+        // if no collisions [0,1] found, then continue on regular path
+        if (c.t == INFINITY){
+            //std::cout << "--mtv-- " << next_pos.x << "," << next_pos.y << "," << next_pos.z << std::endl;
+            return std::pair<std::vector<CollisionData>, glm::vec3>(collisions, next_pos);
+        } else {
+            // nudge along the collision data
+            //std::cout << "COLLIDING......" << std::endl;
+            curr_pos = doNudge(curr_pos, c);
+            glm::vec3 d = next_pos - curr_pos;
+            glm::vec3 d_corrected = (d - glm::dot(d, normals_scale*c.triangle_n)) * (normals_scale*c.triangle_n);
+            next_pos = curr_pos + d_corrected;
+            collisions.push_back(c);
+        }
+
+    }
+
+    return std::pair<std::vector<CollisionData>, glm::vec3>(collisions, curr_pos);
+}
+
+