combine into single header

src/single_header_sph.hpp

@@ -0,0 +1,341 @@
+
+#include "svector.h"
+#include "type.h"
+#include <span>
+#include <tuple>
+#include <vector>
+
+/**
+ * Simplified SPH
+ */
+
+namespace msph {
+
+struct sph_settings {
+  float timestep = 0.00031; // * h / 90;
+  float rest_density = 1000;
+  float visc_constant = 0.0007;
+  float stiffness_k = 1100;
+  float h = 0.051; // smoothing radius
+};
+
+struct domain {
+  float room_width = 0.2;
+  float origin = 5;
+};
+
+struct sim_domain {
+  float origin;
+  float room_width;
+};
+
+class sph_data {
+private:
+  std::vector<particle> particles;
+  struct cell_count {
+    unsigned count;
+    /* to ensure each cell only gets checked once */
+    bool checked = 0;
+  };
+
+  // radix sorted
+  std::vector<cell_count> cell_counts{};
+
+  // map from hash to particle index
+  std::vector<unsigned> sorted_particles{};
+  size_t backing_array_size;
+
+  // grid diameter
+  float diameter;
+  float inv_diameter;
+
+  unsigned hash_func(Vec3 v) {
+    return abs((92837111 * (int)(v.x * inv_diameter)) ^
+               (689287499 * (int)(v.y * inv_diameter)) ^
+               (283923481 * (int)(v.z * inv_diameter)));
+  }
+
+  unsigned cell_id(Vec3 v) { return hash_func(v) % backing_array_size; }
+
+public:
+  sph_data(float diameter, size_t num_particles)
+      : diameter(diameter), inv_diameter(1 / diameter),
+        backing_array_size(2 * num_particles) {}
+  std::vector<particle> &get_particles() { return particles; }
+
+  int repair_map() {
+    // https://matthias-research.github.io/pages/tenMinutePhysics/11-hashing.pdf
+
+    cell_counts.clear();
+    cell_counts.resize(backing_array_size + 1, {0, 0});
+    sorted_particles.resize(backing_array_size);
+
+    for (auto p : particles) {
+      cell_counts[cell_id(p.pos)].count++;
+    }
+
+    /* extra + 1 to account for reading off the end when getting the
+     * next cell */
+    for (int i = 1; i < backing_array_size + 1; i++) {
+      cell_counts[i].count += cell_counts[i - 1].count;
+    }
+
+    for (unsigned int i = 0; i < particles.size(); i++) {
+      unsigned x = --cell_counts[cell_id(particles[i].pos)].count;
+      sorted_particles[x] = i;
+    }
+    /* each element in cell_counts which corresponds to an existing cell
+     * is equal to the cumulative sum of the particles before it, ie it
+     * is equal to the position this cell starts at in the sorted
+     * particle vector */
+
+    return 0;
+  }
+
+  /*
+   * Calls to this function are parallelised.
+   */
+  void get_locality(Vec3 pos, const float support_radius_squared,
+                    std::vector<particle> &loc) {
+    loc.clear();
+    Vec3 center;
+
+    // only check each cell once
+    std::vector<bool> cell_checked{};
+    cell_checked.resize(backing_array_size + 1, false);
+
+    center.x = ((int)(pos.x / diameter)) * diameter + diameter / 2;
+    center.y = ((int)(pos.y / diameter)) * diameter + diameter / 2;
+    center.z = ((int)(pos.z / diameter)) * diameter + diameter / 2;
+
+    constexpr int pmo[] = {1, 0, -1};
+    int discarded = 0;
+
+    for (int i = 0; i < 3; i++) {
+      for (int j = 0; j < 3; j++) {
+        for (int k = 0; k < 3; k++) {
+          auto o = center + Vec3{diameter * pmo[i], diameter * pmo[j],
+                                 diameter * pmo[k]};
+          unsigned int cell = (cell_id(o));
+
+          if (cell_checked[cell]) {
+            continue;
+          }
+
+          cell_checked[cell] = true;
+
+          auto start = cell_counts[cell].count;
+          auto end = cell_counts[cell + 1].count;
+
+          for (auto i = start; i < end; i++) {
+
+            auto p = particles[sorted_particles[i]];
+            Vec3 xij = {pos.x - p.pos.x, pos.y - p.pos.y, pos.z - p.pos.z};
+            if (Vec3_dot(xij, xij) <= support_radius_squared) {
+              loc.emplace_back(p);
+            } else {
+              discarded++;
+            }
+          }
+        }
+      }
+    }
+  }
+};
+
+struct wcsph_solver {
+private:
+  float timestep; // * h / 90;
+  float rest_density;
+  float visc_constant;
+  float stiffness_k;
+  float h; // smoothing radius
+  float particle_mass;
+  float invh;
+  float invmass; // 1/mass
+  float inv_restdenssity;
+  float support_radius;
+  float support_radius_squared;
+  float rev = -1.0; // collision damping
+  sim_domain domain;
+
+  float spiky_kernel_dfdq(float q) {
+    if (0 <= q && q < 1) {
+      return (3.0 / (2 * M_PI)) * (-2 * q + 0.5 * 3 * q * q);
+    } else if (q <= 2) {
+      return (3.0 / (2 * M_PI)) * (-1.0 / 2 * (2 - q) * (2 - q));
+    } else {
+      return 0;
+    }
+  }
+
+  Vec3 gradW_spiky(Vec3 xi, Vec3 xj) {
+    Vec3 dir{xi.x - xj.x, xi.y - xj.y, xi.z - xj.z};
+
+    float omag = mod_vec3(dir);
+    float mag = spiky_kernel_dfdq(omag * invh) * (invh * invh * invh * invh);
+    if (omag != 0) {
+      mag /= omag;
+    } else {
+      mag = 0;
+    }
+
+    dir.x = dir.x * mag;
+    dir.y = dir.y * mag;
+    dir.z = dir.z * mag;
+
+    return dir;
+  }
+
+  float pressure(float density) {
+    /* see test case "pow vs multiply" in sphash-test.cpp */
+    float d = density * inv_restdenssity;
+    return stiffness_k * (d * d * d * d * d * d * d - 1);
+  }
+
+  float spiky_kernel_f(float q) {
+    float s = 3.0 / (2 * M_PI);
+    if (0 <= q && q < 1) {
+      s *= 2.0 / 3 - q * q + 0.5 * q * q * q;
+    } else if (q >= 1 && q < 2) {
+      s *= 1.0 / 6 * (2 - q) * (2 - q) * (2 - q);
+    } else {
+      s *= 0;
+    }
+    return s;
+  }
+
+  float W_spiky_kernel(Vec3 xi, Vec3 xj) {
+    float q = mod_vec3(Vec3{xi.x - xj.x, xi.y - xj.y, xi.z - xj.z}) * invh;
+    return spiky_kernel_f(q) * (invh * invh * invh);
+  }
+
+  std::pair<Vec3, Vec3>
+  update_denspres_gradpres_laplvel(particle &i,
+                                   const std::vector<particle> &locals) {
+    /*
+     * Calculates the density and pressure, then the pressure gradient and
+     * viscosity Laplacian in the same loop to hopefully improve computation
+     * time and take advantage of cache locality.
+     *
+     * Complexity: 2 * local.size()
+     *
+     * Returns a pair; pressure grad, velocity lapl.
+     */
+    float density = 0;
+
+    for (const auto &j : locals) {
+      density += particle_mass * W_spiky_kernel(i.pos, j.pos);
+    }
+
+    i.density = density;
+    i.pressure = pressure(density);
+    Vec3 lapl_vel{0, 0, 0};
+    Vec3 grad_pres{0, 0, 0};
+
+    for (int j = 0; j < locals.size(); j++) {
+      float inv_dens_sq = 1.0 / (locals[j].density * locals[j].density);
+      auto gradW = gradW_spiky(i.pos, locals[j].pos);
+      float temp = particle_mass * (i.pressure * inv_dens_sq +
+                                    (locals[j].pressure * inv_dens_sq));
+      // update
+      grad_pres = grad_pres + gradW * temp;
+
+      Vec3 vij = i.vel - locals[j].vel;
+      Vec3 xij = i.pos - locals[j].pos;
+
+      // update
+      lapl_vel =
+          lapl_vel +
+          2 * vij *
+              (((particle_mass / locals[j].density) * (Vec3_dot(xij, gradW))) /
+               (Vec3_dot(xij, xij) + 0.01 * h * h));
+    }
+
+    return {grad_pres, lapl_vel};
+  }
+
+public:
+  wcsph_solver(const sph_settings s, const sim_domain d)
+      : timestep(s.timestep), rest_density(s.rest_density),
+        visc_constant(s.visc_constant), stiffness_k(s.stiffness_k), h(s.h),
+        particle_mass(h * h * h * rest_density), invh(1 / h),
+        invmass(1 / particle_mass), inv_restdenssity(1 / rest_density),
+        support_radius(2 * h),
+        support_radius_squared(support_radius * support_radius), domain(d) {}
+
+  void update(sph_data &data) {
+    std::vector<std::vector<particle>> locals;
+    int rep = data.repair_map();
+
+    auto &particles = data.get_particles();
+
+    for (int i = 0; i < particles.size(); i++) {
+      auto &p = particles[i];
+      thread_local std::vector<particle> locality;
+      data.get_locality(p.pos, support_radius_squared, locality);
+
+      particle old_p = p;
+      particle new_p = old_p;
+
+      std::pair<Vec3, Vec3> gradp_laplvel =
+          update_denspres_gradpres_laplvel(new_p, locality);
+      Vec3 Fp = -particle_mass * gradp_laplvel.first;
+      Vec3 Fvisc = particle_mass * visc_constant * gradp_laplvel.second;
+
+      Vec3 Fother = Vec3{0, 0, (float)-9.8 * particle_mass};
+
+      Vec3 forces = {
+          Fvisc.x + Fp.x + Fother.x,
+          Fvisc.y + Fp.y + Fother.y,
+          Fvisc.z + Fp.z + Fother.z,
+      };
+
+      new_p.vel = new_p.vel + timestep * invmass * forces;
+      new_p.pos = new_p.pos + timestep * new_p.vel;
+
+      // handle wall collisions
+      if (new_p.pos.z < domain.origin) {
+        new_p.pos.z = domain.origin + (domain.origin - new_p.pos.z);
+        new_p.vel.z *= rev * 0.5;
+      }
+
+      if (new_p.pos.x < domain.origin) {
+        new_p.vel.x *= rev;
+        new_p.pos.x = domain.origin + (domain.origin - new_p.pos.x);
+      }
+      if (new_p.pos.x > domain.room_width) {
+        new_p.pos.x = domain.room_width - (new_p.pos.x - domain.room_width);
+        new_p.vel.x *= rev;
+      }
+      if (new_p.pos.y < domain.origin) {
+        new_p.vel.y *= rev;
+        new_p.pos.y = domain.origin + (domain.origin - new_p.pos.y);
+      }
+      if (new_p.pos.y > domain.room_width) {
+        new_p.vel.y *= rev;
+        new_p.pos.y = domain.room_width - (new_p.pos.y - domain.room_width);
+      }
+
+      float actwidth = domain.room_width - domain.origin;
+      float blocwidth = actwidth / 7;
+      float blocstart = domain.origin + actwidth / 2;
+      float blocend = blocstart + blocwidth;
+
+      if (new_p.pos.y > blocend && new_p.pos.x > blocend) {
+        if (blocend - new_p.pos.x > blocend - p.pos.y) {
+          new_p.pos.x = blocend;
+          new_p.vel.x *= rev;
+        } else {
+          new_p.pos.y = blocend;
+          new_p.vel.y *= rev;
+        }
+      }
+
+      p = new_p;
+    }
+    return;
+  }
+};
+
+}; // namespace msph

src/sph.hpp

@@ -3,11 +3,10 @@
 
 #include "svector.h"
 #include "type.h"
-#include <vector>
-#include <random>
 #include <immintrin.h>
 #include <iostream>
-
+#include <random>
+#include <vector>
 
 namespace sph {
 
@@ -19,133 +18,109 @@ extern float invh;
 extern float inv_restdenssity;
 extern float h;
 
-template <typename T>
-void setup_particles(T &particles, int n) {
-//#define randn() ((rand() % 4096) / 40960.0)
-#define randnosph() 0.2 * h * (float)((float)rand() / (float)RAND_MAX)
-
-    float spacing = 1.0;
-    float a = actual_room_width / h;
-    float b = 0.2 / h;
-    float c = actual_room_width / (2 * h);
-
-    int p, i, j, k;
-    i = 0;
-    p = 0;
-    k = 0;
-    for (int i = 0; i < c || p < n; i++) {
-        for (int j = 0; j < a; j++) {
-            for (int k = 0; k < b; k++) {
-                if (p++ >= n) return;
-                particles.push_back(particle {
-                Vec3 {origin + (spacing * h) * i + randnosph(), 
-                      origin + (spacing * h) * j + randnosph(), 
-                     origin + (spacing * h) * (k)}, 
-                Vec3 {},
-                0,
-                rest_density});
-            }
-        }
-
+template <typename T> void setup_particles(T &particles, int n) {
+// #define randn() ((rand() % 4096) / 40960.0)
+#define randnosph() 0.2 * h *(float)((float)rand() / (float)RAND_MAX)
+
+  float spacing = 1.0;
+  float a = actual_room_width / h;
+  float b = 0.2 / h;
+  float c = actual_room_width / (2 * h);
+
+  int p, i, j, k;
+  i = 0;
+  p = 0;
+  k = 0;
+  for (int i = 0; i < c || p < n; i++) {
+    for (int j = 0; j < a; j++) {
+      for (int k = 0; k < b; k++) {
+        if (p++ >= n)
+          return;
+        particles.push_back(
+            particle{Vec3{origin + (spacing * h) * i + randnosph(),
+                          origin + (spacing * h) * j + randnosph(),
+                          origin + (spacing * h) * (k)},
+                     Vec3{}, 0, rest_density});
+      }
     }
-    std::cout << "num particles added : " << p << "\n";
+  }
+  std::cout << "num particles added : " << p << "\n";
 #undef randnosph
 }
 
-
-
-
 void update_particle_position(particle &p, Vec3 Fvisc, Vec3 Fp);
 
-float step(std::vector<particle> &particles) ;
+float step(std::vector<particle> &particles);
 Vec3 grad_pressure(particle i, const std::vector<particle> &locals);
 
-
-__m256 simd_W_spiky_kernel(
-        __m256 ax, 
-        __m256 ay,
-        __m256 az,
-        __m256 bx, 
-        __m256 by,
-        __m256 bz);
+__m256 simd_W_spiky_kernel(__m256 ax, __m256 ay, __m256 az, __m256 bx,
+                           __m256 by, __m256 bz);
 
 __m256 simd_spiky_kernel_dfdq(__m256 q);
 float spiky_kernel_dfdq(float q);
 
-pair<Vec3> 
-simd_update_denspres_gradpres_laplvel(particle &p, 
-        const particle_locality &locality);
+pair<Vec3>
+simd_update_denspres_gradpres_laplvel(particle &p,
+                                      const particle_locality &locality);
 
 float spiky_kernel_f(float q);
-__m256 simd_spiky_kernel_f(const __m256 q) ;
-Vec3 gradW_spiky (Vec3 xi, Vec3 xj);
+__m256 simd_spiky_kernel_f(const __m256 q);
+Vec3 gradW_spiky(Vec3 xi, Vec3 xj);
 float W_spiky_kernel(Vec3 xi, Vec3 xj);
 
 std::vector<particle> get_local_bf(std::vector<particle> particles,
-        float support_radius, int part) ;
-
-
+                                   float support_radius, int part);
 
 float pressure(float density);
 __m256 simd_pressure(__m256 density);
-void simd_gradW_spiky (
-        __m256 &ox, // output vector
-        __m256 &oy,
-        __m256 &oz,
-        const __m256 &ax, 
-        const __m256 &ay,
-        const __m256 &az,
-        const __m256 &bx, 
-        const __m256 &by,
-        const __m256 &bz) ;
+void simd_gradW_spiky(__m256 &ox, // output vector
+                      __m256 &oy, __m256 &oz, const __m256 &ax,
+                      const __m256 &ay, const __m256 &az, const __m256 &bx,
+                      const __m256 &by, const __m256 &bz);
 
 class update_particle_density {
-    public:
-    float density;
-    Vec3 pos;
-
-    update_particle_density(particle &p) : pos(p.pos) {
-        density = 0;
-    }
-
-    /* Called once for each adjacent particle 
-     *
-     * \param p: the adjacent particle
-     */
-    inline void operator()(particle &j) {
-        density += mass * W_spiky_kernel(pos, j.pos);
-    }
+public:
+  float density;
+  Vec3 pos;
+
+  update_particle_density(particle &p) : pos(p.pos) { density = 0; }
+
+  /* Called once for each adjacent particle
+   *
+   * \param p: the adjacent particle
+   */
+  inline void operator()(particle &j) {
+    density += mass * W_spiky_kernel(pos, j.pos);
+  }
 };
 
-
 class update_particle_forces {
-   public:
-    const particle &i;
-    Vec3 grad_pres {0,0,0};
-    Vec3 lapl_vel {0,0,0};
-
-    /* p must already have correct density and pressure */
-    update_particle_forces(particle &p) : i(p) {}
-
-    /* Called once for each adjacent particle 
-     *
-     * \param p: the adjacent particle
-     */
-    inline void operator()(particle &j) {
-        float inv_dens_sq = 1.0/(j.density * j.density);
-        // last frame density/pressure on locals j
-        auto gradW = gradW_spiky(i.pos , j.pos);
-        float temp = mass * (i.pressure * inv_dens_sq  + (j.pressure * inv_dens_sq));
-        grad_pres = grad_pres + gradW * temp;
-
-        Vec3 vij = i.vel - j.vel;
-        Vec3 xij = i.pos - j.pos;
-
-        lapl_vel = lapl_vel + 2 * (i.vel - j.vel) * 
-            (((mass / j.density) * (Vec3_dot(xij, gradW))) / 
-             (Vec3_dot(xij, xij) + 0.01 * h * h));
-    }
+public:
+  const particle &i;
+  Vec3 grad_pres{0, 0, 0};
+  Vec3 lapl_vel{0, 0, 0};
+
+  /* p must already have correct density and pressure */
+  update_particle_forces(particle &p) : i(p) {}
+
+  /* Called once for each adjacent particle
+   *
+   * \param p: the adjacent particle
+   */
+  inline void operator()(particle &j) {
+    float inv_dens_sq = 1.0 / (j.density * j.density);
+    // last frame density/pressure on locals j
+    auto gradW = gradW_spiky(i.pos, j.pos);
+    float temp = mass * (i.pressure * inv_dens_sq + (j.pressure * inv_dens_sq));
+    grad_pres = grad_pres + gradW * temp;
+
+    Vec3 vij = i.vel - j.vel;
+    Vec3 xij = i.pos - j.pos;
+
+    lapl_vel = lapl_vel + 2 * (i.vel - j.vel) *
+                              (((mass / j.density) * (Vec3_dot(xij, gradW))) /
+                               (Vec3_dot(xij, xij) + 0.01 * h * h));
+  }
 };
 
-
 }; /* namespace sph */