From ad841588ad5ba924e570e91775b2ae4dfc59d769 Mon Sep 17 00:00:00 2001
From: Karma Riuk <riukkarma@gmail.com>
Date: Tue, 25 Apr 2023 12:09:07 +0200
Subject: [PATCH] Refactored collision detection (last version before changing
 the algo)

---
 collisions.cc | 125 ++++++++++++++++++++++----------------------------
 collisions.h  |   2 +-
 2 files changed, 57 insertions(+), 70 deletions(-)

diff --git a/collisions.cc b/collisions.cc
index 747ce1e..3483f3d 100644
--- a/collisions.cc
+++ b/collisions.cc
@@ -14,66 +14,29 @@ static std::vector<vec2d> edges_of(polygon& p) {
     return edges;
 }
 
-static bool separating_axis(
-    vec2d& axis, polygon& p, polygon& q, vec2d* pv, vec2d* impact_point) {
-    double min_p, max_p, min_q, max_q;
-    min_p = min_q = INFINITY;
-    max_p = max_q = -INFINITY;
+struct projection_t {
+    double min_proj, max_proj;
+    vec2d min_point, max_point;
+};
 
-    double projection;
-    vec2d min_p_point, max_p_point;
+static projection_t project(polygon& p, vec2d axis) {
+    projection_t ret{INFINITY, -INFINITY};
+
+    double proj;
     for (auto& point : p.global_points) {
-        projection = vec2d::dot(point, axis);
+        proj = vec2d::dot(point, axis);
 
-        if (projection < min_p) {
-            min_p = projection;
-            min_p_point = point;
+        if (proj < ret.min_proj) {
+            ret.min_proj = proj;
+            ret.min_point = point;
         }
 
-        if (projection > max_p) {
-            max_p = projection;
-            max_p_point = point;
+        if (proj > ret.max_proj) {
+            ret.max_proj = proj;
+            ret.max_point = point;
         }
     }
 
-    for (auto& point : q.global_points) {
-        projection = vec2d::dot(point, axis);
-
-        min_q = std::min(min_q, projection);
-        max_q = std::max(max_q, projection);
-    }
-
-    if (max_p < min_q || max_q < min_p)
-        return true;
-
-    double d;
-    if (max_q - min_p < max_p - min_q) {
-        d = max_q - min_p;
-        *impact_point = min_p_point;
-    } else {
-        d = max_p - min_q;
-        *impact_point = max_p_point;
-    }
-    // push a bit more than needed so the shapes do not overlap in
-    // future tests due to float precision
-    double d_over_o_squared = d / vec2d::dot(axis, axis) + 1e-10;
-    *pv = d_over_o_squared * axis;
-    return false;
-
-    return true;
-}
-
-static uint get_smallest_vec_index(std::vector<vec2d> vs) {
-    uint ret, i = 0;
-    double min = INFINITY;
-    for (auto& v : vs) {
-        double dot = vec2d::dot(v, v);
-        if (dot < min) {
-            ret = i;
-            min = dot;
-        }
-        i++;
-    }
     return ret;
 }
 
@@ -82,40 +45,64 @@ static collision convex_collides(polygon& p, polygon& q) {
 
     std::vector<vec2d> edges_p = edges_of(p);
     std::vector<vec2d> edges_q = edges_of(q);
+
     std::vector<vec2d> edges;
     edges.reserve(edges_p.size() + edges_q.size());
     edges.insert(edges.end(), edges_p.begin(), edges_p.end());
     edges.insert(edges.end(), edges_q.begin(), edges_q.end());
+
     std::vector<vec2d> orthogonals;
     orthogonals.reserve(edges.size());
     for (auto& v : edges)
         orthogonals.push_back(v.orthogonal());
 
-    std::vector<vec2d> push_vectors;
-    push_vectors.reserve(orthogonals.size());
-    std::vector<vec2d> impact_points;
-    push_vectors.reserve(orthogonals.size());
+    std::vector<collision> candidate_collisions;
+    candidate_collisions.reserve(orthogonals.size());
 
-    vec2d push_vector;
-    vec2d impact_point;
+    vec2d d = q.centroid() - p.centroid();
+
+    double min_overlap = INFINITY;
     for (auto& o : orthogonals) {
-        if (separating_axis(o, p, q, &push_vector, &impact_point))
+        vec2d axis = vec2d::normalize(o);
+        if (vec2d::dot(d, axis) > 0)
+            axis *= -1;
+
+
+        projection_t p_proj = project(p, axis);
+        projection_t q_proj = project(q, axis);
+        if (p_proj.max_proj < q_proj.min_proj ||
+            q_proj.max_proj < p_proj.min_proj)
             // the axis is separating (the projections don't overlap)
             return ret;
-        push_vectors.push_back(push_vector);
-        impact_points.push_back(impact_point);
+
+
+        double overlap = std::min(p_proj.max_proj, q_proj.max_proj) -
+                         std::max(p_proj.min_proj, q_proj.min_proj);
+
+        if (overlap < min_overlap) {
+            min_overlap = overlap;
+            ret.n = axis;
+
+            ret.impact_point = p_proj.max_proj > q_proj.min_proj
+                                   ? p_proj.min_point
+                                   : q_proj.max_point;
+
+            // if (vec2d::dot(axis, d) > 0) {
+            //     ret.impact_point = p_proj.max_proj > q_proj.min_proj
+            //                            ? q_proj.min_point
+            //                            : q_proj.max_point;
+            // } else {
+            //     ret.impact_point = p_proj.max_proj > q_proj.min_proj
+            //                            ? p_proj.min_point
+            //                            : p_proj.max_point;
+            // }
+        }
     }
     // if no axis is sperating, then they must be colliding
     ret.collides = true;
-    uint i = get_smallest_vec_index(push_vectors);
-    ret.n = vec2d::normalize(push_vectors[i]);
-    ret.impact_point = impact_points[i];
-
-    // assert that p pushes away from q
-    vec2d d = q.centroid() - p.centroid();
-    if (vec2d::dot(ret.n, d) > 0)
-        ret.n *= -1;
 
+    // if (vec2d::dot(d, ret.n) > 0)
+    //     ret.n *= -1;
     return ret;
 }
 
diff --git a/collisions.h b/collisions.h
index 10b8aa2..83d6441 100644
--- a/collisions.h
+++ b/collisions.h
@@ -5,7 +5,7 @@
 #include "vec2d.h"
 
 struct collision {
-    bool collides;
+    bool collides = false;
     vec2d n; // minimum push vector
     vec2d impact_point;
 };