From 8e91d54de77d13a6606cb01580cfc4c83056d228 Mon Sep 17 00:00:00 2001
From: Florian Sylvain <sylvainflorian@proton.me>
Date: Sun, 11 May 2025 03:08:36 +0200
Subject: [PATCH] feat: collision resolution & spatial grid for balls

---
 src/Ball.cpp      | 30 ++++++++++++++++++++++++++
 src/Ball.hpp      |  1 +
 src/DebugDraw.cpp |  2 +-
 src/Game.cpp      | 55 ++++++++++++++++++++++++++++++++++++++++++++++-
 src/Game.hpp      | 13 +++++++++++
 5 files changed, 99 insertions(+), 2 deletions(-)

diff --git a/src/Ball.cpp b/src/Ball.cpp
index 764fcad..1be7adb 100644
--- a/src/Ball.cpp
+++ b/src/Ball.cpp
@@ -79,3 +79,33 @@ void Ball::handleWallCollision() {
 
     m_shape.setPosition(pos);
 }
+
+void Ball::resolveCollision(Ball& other) {
+    sf::Vector2f posA = getPosition();
+    sf::Vector2f posB = other.getPosition();
+    sf::Vector2f delta = posB - posA;
+    float dist = std::sqrt(delta.x * delta.x + delta.y * delta.y);
+    float minDist = m_radius + other.m_radius;
+
+    if (dist >= minDist || dist < 1e-6f) return;
+
+    sf::Vector2f normal = delta / dist;
+    float overlap = minDist - dist;
+    m_shape.move(-normal * (overlap / 2.f));
+    other.m_shape.move(normal * (overlap / 2.f));
+
+    sf::Vector2f vA = m_velocity;
+    sf::Vector2f vB = other.m_velocity;
+    float vA_n = vA.x * normal.x + vA.y * normal.y;
+    float vB_n = vB.x * normal.x + vB.y * normal.y;
+
+    float restitution = Constants::RESTITUTION;
+    float vA_n_new = vB_n * restitution;
+    float vB_n_new = vA_n * restitution;
+
+    m_velocity += (vA_n_new - vA_n) * normal;
+    other.m_velocity += (vB_n_new - vB_n) * normal;
+
+    m_atRest = false;
+    other.m_atRest = false;
+}
diff --git a/src/Ball.hpp b/src/Ball.hpp
index de11682..fb28d17 100644
--- a/src/Ball.hpp
+++ b/src/Ball.hpp
@@ -21,6 +21,7 @@ class Ball : public PhysicalObject {
     void update(float dt) override;
     void draw(sf::RenderWindow& window) override;
     void applyImpulse(const sf::Vector2f& impulse) override;
+    void resolveCollision(Ball& other);
 
     sf::Vector2f getPosition() const;
     sf::Vector2f getVelocity() const;
diff --git a/src/DebugDraw.cpp b/src/DebugDraw.cpp
index 75108cd..ced5f4c 100644
--- a/src/DebugDraw.cpp
+++ b/src/DebugDraw.cpp
@@ -37,7 +37,7 @@ void DebugDraw::addVelocityLine(sf::VertexArray& lines, const Ball* ball) {
     float velLength = VectorMath::length(velocity);
     float clampedLength = std::max(0.f, std::min(velLength, 100.f));
 
-    addLine(lines, ballCenter, velocity, clampedLength, sf::Color::Blue);
+    addLine(lines, ballCenter, velocity, clampedLength, sf::Color::Red);
 }
 
 void DebugDraw::drawBatchedLines(sf::RenderWindow& window,
diff --git a/src/Game.cpp b/src/Game.cpp
index a1859c1..d51c180 100644
--- a/src/Game.cpp
+++ b/src/Game.cpp
@@ -1,7 +1,10 @@
 #include "Game.hpp"
 
 #include <SFML/Window/Event.hpp>
+#include <algorithm>
 #include <cmath>
+#include <tuple>
+#include <unordered_map>
 
 #include "BallFactory.hpp"
 #include "Constants.hpp"
@@ -57,9 +60,59 @@ void Game::handleMouseClick(const sf::Vector2i &mousePos) {
     }
 }
 
+Game::Grid Game::buildSpatialGrid() {
+    const float cellSize = 2 * Constants::BALL_RADIUS;
+    const float safeCellSize = std::max(cellSize, 0.001f);
+    Grid grid;
+    for (auto &object : m_objects) {
+        if (Ball *ball = dynamic_cast<Ball *>(object.get())) {
+            const sf::Vector2f &pos = ball->getPosition();
+            if (std::isfinite(pos.x) && std::isfinite(pos.y)) {
+                int cellX = static_cast<int>(std::floor(pos.x / safeCellSize));
+                int cellY = static_cast<int>(std::floor(pos.y / safeCellSize));
+                grid[{cellX, cellY}].push_back(ball);
+            }
+        }
+    }
+    return grid;
+}
+
+void Game::resolveSpatialCollisions(const Grid &grid) {
+    static const Cell forwardNeighbors[] = {
+        {0, 0}, {1, 0}, {1, 1}, {0, 1}, {-1, 1}};
+    for (const auto &[cell, cellBalls] : grid) {
+        for (const auto &offset : forwardNeighbors) {
+            Cell neighborCell = {cell.first + offset.first,
+                                 cell.second + offset.second};
+            auto neighborIt = grid.find(neighborCell);
+            if (neighborIt == grid.end()) continue;
+            if (neighborCell == cell) {
+                for (size_t i = 0; i < cellBalls.size(); ++i) {
+                    for (size_t j = i + 1; j < cellBalls.size(); ++j) {
+                        cellBalls[i]->resolveCollision(*cellBalls[j]);
+                    }
+                }
+            } else {
+                for (Ball *ballA : cellBalls) {
+                    for (Ball *ballB : neighborIt->second) {
+                        ballA->resolveCollision(*ballB);
+                    }
+                }
+            }
+        }
+    }
+}
+
 void Game::update() {
     float dt = m_clock.restart().asSeconds() * m_timeScale;
-    for (auto &object : m_objects) object->update(dt);
+    if (dt > 0.1f) dt = 0.1f;
+
+    for (auto &object : m_objects) {
+        object->update(dt);
+    }
+
+    auto grid = buildSpatialGrid();
+    resolveSpatialCollisions(grid);
 }
 
 void Game::render() {
diff --git a/src/Game.hpp b/src/Game.hpp
index 0df933a..1ca7068 100644
--- a/src/Game.hpp
+++ b/src/Game.hpp
@@ -7,6 +7,7 @@
 #include <SFML/System/Vector2.hpp>
 #include <SFML/Window/Event.hpp>
 #include <memory>
+#include <unordered_map>
 #include <vector>
 
 #include "Ball.hpp"
@@ -35,6 +36,18 @@ class Game {
     void update();
     void render();
 
+    struct CellHash {
+        std::size_t operator()(const std::pair<int, int>& k) const {
+            return static_cast<std::size_t>(k.first) * 73856093 ^
+                   static_cast<std::size_t>(k.second) * 19349663;
+        }
+    };
+
+    using Cell = std::pair<int, int>;
+    using Grid = std::unordered_map<Cell, std::vector<Ball*>, CellHash>;
+    Grid buildSpatialGrid();
+    void resolveSpatialCollisions(const Grid& grid);
+
    public:
     Game();
     void run();