From 66dba151ed102e6215ecf4a8778601415729ad65 Mon Sep 17 00:00:00 2001
From: Klemen Plestenjak <plestenjak.klemen@gmail.com>
Date: Fri, 17 Nov 2023 19:35:20 +0100
Subject: [PATCH] Add optimal update and fix invalid write bugs

---
 engine/breeze/utils/spatial_grid.c | 34 ++++++++++++++++++++++++------
 1 file changed, 28 insertions(+), 6 deletions(-)

diff --git a/engine/breeze/utils/spatial_grid.c b/engine/breeze/utils/spatial_grid.c
index 50312ee..8ea50fd 100644
--- a/engine/breeze/utils/spatial_grid.c
+++ b/engine/breeze/utils/spatial_grid.c
@@ -85,7 +85,7 @@ static void removeCell(BzSpatialGrid *grid, BzSpatialGridID id, i32 x, i32 y) {
     }
     if (i < bzArraySize(*cell)) {
         BzSpatialGridID lastID = bzArrayPop(*cell);
-        (*cell)[id] = lastID;
+        (*cell)[i] = lastID;
     }
 }
 
@@ -125,18 +125,40 @@ BzSpatialGridID bzSpatialGridInsert(BzSpatialGrid *grid, void *data, f32 posX, f
 
     return id;
 }
+
+static bool indexContains(BzSpatialGridIndex index, i32 x, i32 y) {
+    return x >= index.minX && x <= index.maxX && y >= index.minY && y <= index.maxY;
+}
+
 BzSpatialGridID bzSpatialGridUpdate(BzSpatialGrid *grid, BzSpatialGridID id, f32 posX, f32 posY, f32 sizeX, f32 sizeY) {
     BzSpatialGridEntry *entry = bzObjectPoolGetObject(grid->entriesPool, id);
     BZ_ASSERT(entry && entry->id == id);
 
-    BzSpatialGridIndex index = calculateGridIndex(grid, posX, posY, sizeX, sizeY);
-    if (entry->index.minX == index.minX &&
-        entry->index.minY == index.minY &&
-        entry->index.maxX == index.maxX &&
-        entry->index.maxY == index.maxY)  {
+    BzSpatialGridIndex oldIndex = entry->index;
+    BzSpatialGridIndex newIndex = calculateGridIndex(grid, posX, posY, sizeX, sizeY);
+    if (oldIndex.minX == newIndex.minX &&
+        oldIndex.minY == newIndex.minY &&
+        oldIndex.maxX == newIndex.maxX &&
+        oldIndex.maxY == newIndex.maxY)  {
         return id;
     }
 
+    for (i32 y = oldIndex.minY; y <= oldIndex.maxY; y++) {
+        for (i32 x = oldIndex.minX; x <= oldIndex.maxX; x++) {
+            bool inNew = indexContains(newIndex, x, y);
+            if (!inNew) removeCell(grid, id, x, y);
+        }
+    }
+
+    for (i32 y = newIndex.minY; y <= newIndex.maxY; y++) {
+        for (i32 x = newIndex.minX; x <= newIndex.maxX; x++) {
+            bool inOld = indexContains(oldIndex, x, y);
+            if (!inOld) insertCell(grid, id, x, y);
+
+        }
+    }
+
+    entry->index = newIndex;
     return id;
 }
 void bzSpatialGridRemove(BzSpatialGrid *grid, BzSpatialGridID id) {