1 files changed, 66 insertions, 3 deletions

diff --git a/spheres.hpp b/spheres.hpp
index 5ecad39..ef6b665 100644
--- a/spheres.hpp
+++ b/spheres.hpp
@@ -1,7 +1,9 @@
 #pragma once
 
+#include <assert.h>
 #include <set>
 #include <vector>
+#include <queue>
 
 #include <eigen3/Eigen/Dense>
 
@@ -11,6 +13,7 @@
 using Rng = randutils::random_generator<pcg32>;
 
 template <int D> using Vector = Eigen::Matrix<double, D, 1>;
+template <int D> using Matrix = Eigen::Matrix<double, D, D>;
 
 template <int D> class Position : public Vector<D> {
 private:
@@ -58,6 +61,7 @@ public:
 
 template <typename T, int D> concept Particle = requires(T v, const T& w) {
   { v.x } -> std::same_as<Position<D>>;
+  { v.m } -> std::same_as<bool>;
   { v.U(w) } -> std::same_as<double>;
 };
 
@@ -93,6 +97,7 @@ private:
     for (unsigned i = 0; i < D; i++) {
       ind += pow(n, i) * std::floor(n * x(i));
     }
+    assert(ind < lists.size());
     return ind;
   }
 
@@ -155,9 +160,10 @@ public:
 template <int D> class Sphere {
 public:
   Position<D> x;
+  bool m;
   double r;
 
-  Sphere(const Position<D>& x, double r) : x(x), r(r) {};
+  Sphere(const Position<D>& x, double r) : x(x), m(false), r(r) {};
 
   double regularizedDistanceSquared(const Sphere<D>& s) const {
     return (x - s.x).squaredNorm() / pow(r + s.r, 2);
@@ -204,6 +210,24 @@ public:
   }
 };
 
+template <int D> class Euclidean {
+public:
+  Vector<D> t;
+  Matrix<D> r;
+
+  Euclidean() {
+    t.setZero();
+    r.setIdentity();
+  }
+
+  Euclidean(const Vector<D>& t0, const Matrix<D>& r0) : t(t0), r(r0) {}
+
+  Euclidean inverse() const { return Euclidean(-r.transpose() * t, r.transpose()); }
+
+  Vector<D> act(const Vector<D>& x) const { return r * x; }
+  Position<D> act(const Position<D>& x) const { return (Position<D>)(r * x) + t; }
+};
+
 template <int D, Particle<D> T> class Model {
 private:
   NeighborLists<D, T> nl;
@@ -247,16 +271,20 @@ public:
 
   double energyChangeSwap(const T& p1, const T& p2) const {
     double ΔE = 0;
+    T p1New = p1;
+    T p2New = p2;
+    p1New.r = p2.r;
+    p2New.r = p1.r;
     for (const T* neighbor : neighborsOf(p1.x)) {
       if (neighbor != &p1 && neighbor != &p2) {
         ΔE -= neighbor->U(p1);
-        ΔE += neighbor->U({p1.x, p2.r});
+        ΔE += neighbor->U(p1New);
       }
     }
     for (const T* neighbor : neighborsOf(p2.x)) {
       if (neighbor != &p1 && neighbor != &p2) {
         ΔE -= neighbor->U(p2);
-        ΔE += neighbor->U({p2.x, p1.r});
+        ΔE += neighbor->U(p2New);
       }
     }
 
@@ -286,4 +314,39 @@ public:
       return false;
     }
   }
+
+  unsigned clusterFlip(double β, const Euclidean<D>& R, T& p0, Rng& r) {
+    unsigned n = 0;
+    std::queue<T*> q;
+    q.push(&p0);
+
+    while (!q.empty()) {
+      T* p = q.front();
+      q.pop();
+
+      if (!p->m) {
+        p->m = true;
+        T pNew = *p;
+        pNew.x = R.act(p->x);
+
+        for (T* neighbor : neighborsOf(pNew.x)) {
+          if (!neighbor->m) {
+            double ΔE = neighbor->U(pNew) - neighbor->U(*p);
+            if (1 - exp(-β * ΔE) > r.uniform(0.0, 1.0)) {
+              q.push(neighbor);
+            }
+          }
+        }
+
+        move(*p, pNew.x);
+        n++;
+      }
+    }
+
+    for (T& p : particles) {
+      p.m = false;
+    }
+
+    return n;
+  }
 };