implement uniform grid for repulsion forces

include/config.hpp

@@ -21,6 +21,7 @@ constexpr float SPRING_CONSTANT = 1.5;
 constexpr float DAMPENING_CONSTANT = 0.8;
 constexpr float REST_LENGTH = 1.0;
 constexpr float REPULSION_FORCE = 0.05;
+constexpr float REPULSION_RANGE = 3.0 * REST_LENGTH;
 constexpr float VERLET_DAMPENING = 0.01; // [0, 1]
 
 // Graph Drawing

src/main.cpp

@@ -1,6 +1,8 @@
 #define VERLET_UPDATE
 
+#include <chrono>
 #include <iostream>
+#include <ratio>
 #include <raylib.h>
 #include <raymath.h>
 
@@ -11,27 +13,16 @@
 
 auto klotski_a() -> State {
   State s = State(4, 5);
-  Block a = Block(0, 0, 1, 2, false);
+  s.AddBlock(Block(0, 0, 1, 2, false));
-  Block b = Block(1, 0, 2, 2, true);
+  s.AddBlock(Block(1, 0, 2, 2, true));
-  Block c = Block(3, 0, 1, 2, false);
+  s.AddBlock(Block(3, 0, 1, 2, false));
-  Block d = Block(0, 2, 1, 2, false);
+  s.AddBlock(Block(0, 2, 1, 2, false));
-  // Block e = Block(1, 2, 2, 1, false);
+  // s.AddBlock(Block(1, 2, 2, 1, false));
-  // Block f = Block(3, 2, 1, 2, false);
+  // s.AddBlock(Block(3, 2, 1, 2, false));
-  // Block g = Block(1, 3, 1, 1, false);
+  // s.AddBlock(Block(1, 3, 1, 1, false));
-  // Block h = Block(2, 3, 1, 1, false);
+  // s.AddBlock(Block(2, 3, 1, 1, false));
-  // Block i = Block(0, 4, 1, 1, false);
+  // s.AddBlock(Block(0, 4, 1, 1, false));
-  // Block j = Block(3, 4, 1, 1, false);
+  // s.AddBlock(Block(3, 4, 1, 1, false));
-
-  s.AddBlock(a);
-  s.AddBlock(b);
-  s.AddBlock(c);
-  s.AddBlock(d);
-  // s.AddBlock(e);
-  // s.AddBlock(f);
-  // s.AddBlock(g);
-  // s.AddBlock(h);
-  // s.AddBlock(i);
-  // s.AddBlock(j);
 
   return s;
 }
@@ -94,6 +85,12 @@ auto main(int argc, char *argv[]) -> int {
   int hov_y = 0;
   int sel_x = 0;
   int sel_y = 0;
+  double last_print_time = GetTime();
+  std::chrono::duration<double, std::milli> physics_time_accumulator =
+      std::chrono::duration<double, std::milli>(0);
+  std::chrono::duration<double, std::milli> render_time_accumulator =
+      std::chrono::duration<double, std::milli>(0);
+  int time_measure_count = 0;
   while (!WindowShouldClose()) {
     frametime = GetFrameTime();
 
@@ -157,6 +154,8 @@ auto main(int argc, char *argv[]) -> int {
     }
 
     // Physics update
+    std::chrono::high_resolution_clock::time_point ps =
+        std::chrono::high_resolution_clock::now();
     mass_springs.ClearForces();
     mass_springs.CalculateSpringForces();
     mass_springs.CalculateRepulsionForces();
@@ -165,12 +164,34 @@ auto main(int argc, char *argv[]) -> int {
 #else
     mass_springs.EulerUpdate(frametime * SIM_SPEED);
 #endif
+    std::chrono::high_resolution_clock::time_point pe =
+        std::chrono::high_resolution_clock::now();
+    physics_time_accumulator += pe - ps;
 
     // Rendering
+    std::chrono::high_resolution_clock::time_point rs =
+        std::chrono::high_resolution_clock::now();
+    renderer.UpdateCamera();
     renderer.DrawMassSprings(mass_springs);
     renderer.DrawKlotski(board, hov_x, hov_y, sel_x, sel_y);
     renderer.DrawTextures();
-    renderer.UpdateCamera();
+    std::chrono::high_resolution_clock::time_point re =
+        std::chrono::high_resolution_clock::now();
+    render_time_accumulator += re - rs;
+
+    time_measure_count++;
+    if (GetTime() - last_print_time > 3.0) {
+      std::cout << "\n - Physics time avg: "
+                << physics_time_accumulator / time_measure_count << "."
+                << std::endl;
+      std::cout << " - Render time avg:  "
+                << render_time_accumulator / time_measure_count << "."
+                << std::endl;
+      last_print_time = GetTime();
+      physics_time_accumulator = std::chrono::duration<double, std::milli>(0);
+      render_time_accumulator = std::chrono::duration<double, std::milli>(0);
+      time_measure_count = 0;
+    }
   }
 
   CloseWindow();

src/mass_springs.cpp

@@ -3,6 +3,8 @@
 
 #include <format>
 #include <raymath.h>
+#include <unordered_map>
+#include <vector>
 
 auto Mass::ClearForce() -> void { force = Vector3Zero(); }
 
@@ -121,19 +123,84 @@ auto MassSpringSystem::CalculateSpringForces() -> void {
 }
 
 auto MassSpringSystem::CalculateRepulsionForces() -> void {
+  const float INV_CELL = 1.0 / REPULSION_RANGE;
+
+  struct CellKey {
+    int x, y, z;
+    bool operator==(const CellKey &other) const {
+      return x == other.x && y == other.y && z == other.z;
+    }
+  };
+  struct CellHash {
+    size_t operator()(const CellKey &key) const {
+      return ((size_t)key.x * 73856093) ^ ((size_t)key.y * 19349663) ^
+             ((size_t)key.z * 83492791);
+    }
+  };
+
+  // Accelerate with uniform grid
+  std::unordered_map<CellKey, std::vector<Mass *>, CellHash> grid;
+  grid.reserve(masses.size());
+
   for (auto &[state, mass] : masses) {
-    for (auto &[s, m] : masses) {
+    CellKey key{
-      Vector3 dx = Vector3Subtract(mass.position, m.position);
+        (int)std::floor(mass.position.x * INV_CELL),
+        (int)std::floor(mass.position.y * INV_CELL),
+        (int)std::floor(mass.position.z * INV_CELL),
+    };
+    grid[key].push_back(&mass);
+  }
 
-      // This can be accelerated with a spatial data structure
+  for (auto &[state, mass] : masses) {
-      if (Vector3Length(dx) >= 3 * REST_LENGTH) {
+    int cx = (int)std::floor(mass.position.x * INV_CELL);
-        continue;
+    int cy = (int)std::floor(mass.position.y * INV_CELL);
+    int cz = (int)std::floor(mass.position.z * INV_CELL);
+
+    // Check all 27 neighboring cells (including own)
+    for (int dx = -1; dx <= 1; ++dx) {
+      for (int dy = -1; dy <= 1; ++dy) {
+        for (int dz = -1; dz <= 1; ++dz) {
+          CellKey neighbor{cx + dx, cy + dy, cz + dz};
+          auto it = grid.find(neighbor);
+          if (it == grid.end()) {
+            continue;
+          }
+
+          for (Mass *m : it->second) {
+            if (m == &mass) {
+              continue; // skip self
+            }
+
+            Vector3 diff = Vector3Subtract(mass.position, m->position);
+            float len = Vector3Length(diff);
+
+            if (len == 0.0f || len >= REPULSION_RANGE) {
+              continue;
+            }
+
+            mass.force =
+                Vector3Add(mass.force, Vector3Scale(Vector3Normalize(diff),
+                                                    REPULSION_FORCE));
+          }
+        }
       }
-
-      mass.force = Vector3Add(
-          mass.force, Vector3Scale(Vector3Normalize(dx), REPULSION_FORCE));
     }
   }
+
+  // Old method
+  // for (auto &[state, mass] : masses) {
+  //   for (auto &[s, m] : masses) {
+  //     Vector3 dx = Vector3Subtract(mass.position, m.position);
+  //
+  //     // This can be accelerated with a spatial data structure
+  //     if (Vector3Length(dx) >= 3 * REST_LENGTH) {
+  //       continue;
+  //     }
+  //
+  //     mass.force = Vector3Add(
+  //         mass.force, Vector3Scale(Vector3Normalize(dx), REPULSION_FORCE));
+  //   }
+  // }
 }
 
 auto MassSpringSystem::EulerUpdate(float delta_time) -> void {