cleanup repulsion force calculation

include/config.hpp

@@ -37,7 +37,7 @@ constexpr float DAMPENING_CONSTANT = 1.0;
 constexpr float REST_LENGTH = 2.0;
 constexpr float REPULSION_FORCE = 0.1;
 constexpr float REPULSION_RANGE = 5.0 * REST_LENGTH;
-constexpr int REPULSION_GRID_REFRESH = 5; // Frames between grid rebuilds
+constexpr int REPULSION_GRID_REFRESH = 5; // Updates between grid rebuilds
 constexpr float VERLET_DAMPENING = 0.05;  // [0, 1]
 
 // Graph Drawing

include/physics.hpp

@@ -86,8 +86,8 @@ public:
 
 class MassSpringSystem {
 private:
-  std::vector<Mass *> mass_vec;
+  std::vector<Mass *> mass_pointers;
-  std::vector<int> indices;
+  std::vector<int> mass_indices;
   std::vector<int64_t> cell_ids;
   int last_build;
   int last_masses_count;
@@ -110,7 +110,7 @@ public:
   ~MassSpringSystem() {};
 
 private:
-  auto BuildGrid() -> void;
+  auto BuildUniformGrid() -> void;
 
 public:
   auto AddMass(float mass, bool fixed, const State &state) -> void;

include/state.hpp

@@ -2,9 +2,9 @@
 #define __STATE_HPP_
 
 #include "config.hpp"
-#include "puzzle.hpp"
 #include "physics.hpp"
 #include "presets.hpp"
+#include "puzzle.hpp"
 
 #include <raymath.h>
 

src/physics.cpp

@@ -1,6 +1,7 @@
 #include "physics.hpp"
 #include "config.hpp"
 
+#include <algorithm>
 #include <numeric>
 #include <raylib.h>
 #include <raymath.h>
@@ -135,92 +136,104 @@ auto MassSpringSystem::CalculateSpringForces() -> void {
   }
 }
 
-auto MassSpringSystem::BuildGrid() -> void {
+auto MassSpringSystem::BuildUniformGrid() -> void {
-  const float INV_CELL = 1.0f / REPULSION_RANGE;
+  // Collect pointers to all masses
-  const int n = masses.size();
+  mass_pointers.clear();
-
+  mass_pointers.reserve(masses.size());
-  // Collect pointers
-  mass_vec.clear();
-  mass_vec.reserve(n);
   for (auto &[state, mass] : masses) {
-    mass_vec.push_back(&mass);
+    mass_pointers.push_back(&mass);
   }
 
-  // Assign each particle a cell index
+  // Assign each mass a cell_id based on its position.
-  auto cellID = [&](const Vector3 &p) -> int64_t {
+  auto cell_id = [&](const Vector3 &position) -> int64_t {
-    int x = (int)std::floor(p.x * INV_CELL);
+    int x = (int)std::floor(position.x / REPULSION_RANGE);
-    int y = (int)std::floor(p.y * INV_CELL);
+    int y = (int)std::floor(position.y / REPULSION_RANGE);
-    int z = (int)std::floor(p.z * INV_CELL);
+    int z = (int)std::floor(position.z / REPULSION_RANGE);
-    // Pack into a single int64 (assumes coords fit in 20 bits each)
+    // Pack into a single int64 (assumes a coordinate fits in 20 bits)
     return ((int64_t)(x & 0xFFFFF) << 40) | ((int64_t)(y & 0xFFFFF) << 20) |
            (int64_t)(z & 0xFFFFF);
   };
 
-  // Sort particles by cell
+  // Sort mass indices by cell_id to improve cache locality and allow cell
-  indices.clear();
+  // iteration with std::lower_bound and std::upper_bound
-  indices.resize(n);
+  mass_indices.clear();
-  std::iota(indices.begin(), indices.end(), 0);
+  mass_indices.resize(masses.size());
-  std::sort(indices.begin(), indices.end(), [&](int a, int b) {
+  std::iota(mass_indices.begin(), mass_indices.end(),
-    return cellID(mass_vec[a]->position) < cellID(mass_vec[b]->position);
+            0); // Fill the indices array with ascending numbers
+  std::sort(mass_indices.begin(), mass_indices.end(), [&](int a, int b) {
+    return cell_id(mass_pointers[a]->position) <
+           cell_id(mass_pointers[b]->position);
   });
 
-  // Build cell start/end table
+  // Build cell start/end table: maps mass index to cell_id.
+  // All indices of a single cell are consecutive.
   cell_ids.clear();
-  cell_ids.resize(n);
+  cell_ids.resize(masses.size());
-  for (int i = 0; i < n; ++i) {
+  for (int i = 0; i < masses.size(); ++i) {
-    cell_ids[i] = cellID(mass_vec[indices[i]]->position);
+    cell_ids[i] = cell_id(mass_pointers[mass_indices[i]]->position);
   }
 }
 
 auto MassSpringSystem::CalculateRepulsionForces() -> void {
-  const float INV_CELL = 1.0f / REPULSION_RANGE;
+  // Refresh grid if necessary
-  const int n = masses.size();
-
   if (last_build >= REPULSION_GRID_REFRESH ||
       masses.size() != last_masses_count ||
       springs.size() != last_springs_count) {
-    BuildGrid();
+    BuildUniformGrid();
     last_build = 0;
     last_masses_count = masses.size();
     last_springs_count = springs.size();
   }
   last_build++;
 
+  // TODO: Use Barnes-Hut + Octree
 #pragma omp parallel for
-  for (int i = 0; i < n; ++i) {
+  // Search the neighboring cells for each mass to calculate repulsion forces
-    Mass *mass = mass_vec[indices[i]];
+  for (int i = 0; i < masses.size(); ++i) {
-    int cx = (int)std::floor(mass->position.x * INV_CELL);
+    Mass *mass = mass_pointers[mass_indices[i]];
-    int cy = (int)std::floor(mass->position.y * INV_CELL);
+    int cell_x = (int)std::floor(mass->position.x / REPULSION_RANGE);
-    int cz = (int)std::floor(mass->position.z * INV_CELL);
+    int cell_y = (int)std::floor(mass->position.y / REPULSION_RANGE);
+    int cell_z = (int)std::floor(mass->position.z / REPULSION_RANGE);
 
-    Vector3 force = {0, 0, 0};
+    Vector3 force = Vector3Zero();
 
-    // Search all 3*3*3 neighbor cells for particles
+    // Search all 3*3*3 neighbor cells for masses
     for (int dx = -1; dx <= 1; ++dx) {
       for (int dy = -1; dy <= 1; ++dy) {
         for (int dz = -1; dz <= 1; ++dz) {
-          int64_t nid = ((int64_t)((cx + dx) & 0xFFFFF) << 40) |
+          int64_t neighbor_id = ((int64_t)((cell_x + dx) & 0xFFFFF) << 40) |
-                        ((int64_t)((cy + dy) & 0xFFFFF) << 20) |
+                                ((int64_t)((cell_y + dy) & 0xFFFFF) << 20) |
-                        (int64_t)((cz + dz) & 0xFFFFF);
+                                (int64_t)((cell_z + dz) & 0xFFFFF);
 
-          // Binary search for this neighbor cell in sorted array
+          // Find the first and last occurence of the neighbor_id (iterator).
-          auto lo = std::lower_bound(cell_ids.begin(), cell_ids.end(), nid);
+          // Because cell_ids is sorted, all elements of this cell are between
-          auto hi = std::upper_bound(cell_ids.begin(), cell_ids.end(), nid);
+          // those.
+          // If there is no cell, the iterators just won't do anything.
+          auto cell_start =
+              std::lower_bound(cell_ids.begin(), cell_ids.end(), neighbor_id);
+          auto cell_end =
+              std::upper_bound(cell_ids.begin(), cell_ids.end(), neighbor_id);
 
-          for (auto it = lo; it != hi; ++it) {
+          // For each mass, iterate through all the masses of neighboring cells
-            Mass *m = mass_vec[indices[it - cell_ids.begin()]];
+          // to accumulate the repulsion forces.
-            if (m == mass) {
+          // This is slow with O(n * m), where m is the number of masses in each
+          // neighboring cell.
+          for (auto it = cell_start; it != cell_end; ++it) {
+            Mass *neighbor = mass_pointers[mass_indices[it - cell_ids.begin()]];
+            if (neighbor == mass) {
+              // Skip ourselves
               continue;
             }
 
-            Vector3 diff = Vector3Subtract(mass->position, m->position);
+            Vector3 direction =
-            float len = Vector3Length(diff);
+                Vector3Subtract(mass->position, neighbor->position);
-            if (len == 0.0f || len >= REPULSION_RANGE) {
+            float distance = Vector3Length(direction);
+            if (distance == 0.0f || distance >= REPULSION_RANGE) {
               continue;
             }
 
-            force = Vector3Add(
+            force = Vector3Add(force, Vector3Scale(Vector3Normalize(direction),
-                force, Vector3Scale(Vector3Normalize(diff), REPULSION_FORCE));
+                                                   REPULSION_FORCE));
           }
         }
       }
@@ -237,8 +250,8 @@ auto MassSpringSystem::VerletUpdate(float delta_time) -> void {
 }
 
 auto MassSpringSystem::InvalidateGrid() -> void {
-  mass_vec.clear();
+  mass_pointers.clear();
-  indices.clear();
+  mass_indices.clear();
   cell_ids.clear();
   last_build = REPULSION_GRID_REFRESH;
   last_masses_count = 0;

src/renderer.cpp

@@ -94,6 +94,10 @@ auto Renderer::DrawMassSprings(const MassSpringSystem &mass_springs,
   rlEnd();
 
   // Draw masses (instanced)
+  // NOTE: I don't know if drawing all this inside a shader would make it much
+  //       faster...
+  //       The amount of data sent to the GPU would be reduced (just positions
+  //       instead of matrices), but is this noticable for < 100000 cubes?
   DrawMeshInstanced(cube_instance, vertex_mat, transforms,
                     mass_springs.masses.size());
 
@@ -118,7 +122,9 @@ auto Renderer::DrawMassSprings(const MassSpringSystem &mass_springs,
   DrawCube(current_mass.position, VERTEX_SIZE * 2, VERTEX_SIZE * 2,
            VERTEX_SIZE * 2, RED);
 
-  // DrawGrid(10, 1.0);
+  // DrawCubeWires(current_mass.position, REPULSION_RANGE, REPULSION_RANGE,
+  //               REPULSION_RANGE, BLACK);
+  // DrawGrid(100, 1.0);
   // DrawSphere(camera.target, VERTEX_SIZE, ORANGE);
   EndMode3D();