3 files changed, 101 insertions, 13 deletions

diff --git a/hard_spheres.cpp b/hard_spheres.cpp
index b53f7cf..6a14953 100644
--- a/hard_spheres.cpp
+++ b/hard_spheres.cpp
@@ -29,10 +29,23 @@ int main(int argc, char* argv[]) {
 
     HardSphere<2>& s1 = r.pick(m.particles);
     HardSphere<2>& s2 = r.pick(m.particles);
-    
-    m.swap(1, s1, s2, r);
 
-    if (i % (N * 3) == 0) {
+    Vector<2> t1 = s1.x;
+    Vector<2> t2 = s2.x;
+    Vector<2> t = (t1 + t2) / 2;
+
+    Matrix<2> mat;
+
+    mat(0, 0) = -1;
+    mat(1, 1) = -1;
+    mat(0, 1) = 0;
+    mat(1, 0) = 0;
+
+    Euclidean<2> g(t - mat * t, mat);
+
+    std::cout << m.clusterFlip(1, g, s1, r) << std::endl;
+
+    if (i % (N) == 0) {
       draw(m);
     }
   }
diff --git a/soft_spheres.cpp b/soft_spheres.cpp
index a71a472..cdd6f31 100644
--- a/soft_spheres.cpp
+++ b/soft_spheres.cpp
@@ -4,7 +4,7 @@ int main(int argc, char* argv[]) {
   const unsigned n = 12;
 
   unsigned N = 1200;
-  double R = 0.024;
+  double R = 0.023;
   double ε = 0.01;
   unsigned steps = 1e6;
   double β = 1;
@@ -28,16 +28,28 @@ int main(int argc, char* argv[]) {
       Δxi = r.variate<double, std::normal_distribution>(0, ε);
     }
 
-    m.metropolis(β, s, Δx, r);
+    for (unsigned j = 0; j < N; j++)
+      m.metropolis(β, s, Δx, r);
 
     SoftSphere<2, n>& s1 = r.pick(m.particles);
     SoftSphere<2, n>& s2 = r.pick(m.particles);
-    
-    m.swap(β, s1, s2, r);
 
-    if (i % (N * 3) == 0) {
-      draw(m);
-    }
+    Vector<2> t1 = s1.x;
+    Vector<2> t2 = s2.x;
+    Vector<2> t = (t1 + t2) / 2;
+
+    Matrix<2> mat;
+
+    mat(0, 0) = -1;
+    mat(1, 1) = -1;
+    mat(0, 1) = 0;
+    mat(1, 0) = 0;
+
+    Euclidean<2> g(t - mat * t, mat);
+
+    std::cout << m.clusterFlip(1, g, s1, r) << std::endl;
+    
+    draw(m);
   }
 
   return 0;
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;
+  }
 };