From efad10c1fbcdab34e50f577ca0478de5632c8bbc Mon Sep 17 00:00:00 2001
From: Jaron Kent-Dobias <jaron@kent-dobias.com>
Date: Thu, 29 Sep 2022 17:35:53 +0200
Subject: Working implementation of the hungarian algorithm.

---
 hungarian.cpp | 283 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 283 insertions(+)
 create mode 100644 hungarian.cpp

diff --git a/hungarian.cpp b/hungarian.cpp
new file mode 100644
index 0000000..00cc91d
--- /dev/null
+++ b/hungarian.cpp
@@ -0,0 +1,283 @@
+#include <iostream>
+#include <cmath>
+#include <functional>
+#include <random>
+#include <vector>
+#include <limits>
+#include <queue>
+#include <stack>
+
+#include "randutils/randutils.hpp"
+#include "pcg-cpp/include/pcg_random.hpp"
+
+using Rng = randutils::random_generator<pcg32>;
+
+class Edge;
+
+class Vertex {
+public:
+  unsigned index;
+  double y;
+  bool inZ;
+  std::vector<std::reference_wrapper<Edge>> neighbors;
+  std::array<unsigned, 2> coordinate;
+
+  Vertex() : y(0), inZ(false) {};
+
+  void addEdge(Edge& e) {
+    neighbors.push_back(e);
+  }
+
+  bool inMatching() const;
+};
+
+class Edge {
+public:
+  Vertex* s;
+  Vertex* t;
+  bool orientation;
+  double weight;
+  bool inPath;
+
+  Edge() : orientation(false) {};
+  void setVertices(Vertex& red, Vertex& blue) {
+    s = &red;
+    t = &blue;
+    red.addEdge(*this);
+    blue.addEdge(*this);
+  }
+  Vertex& tail()  {
+    if (orientation) {
+      return *t;
+    } else {
+      return *s;
+    }
+  }
+  Vertex& head() {
+    if (orientation) {
+      return *s;
+    } else {
+      return *t;
+    }
+  }
+  const Vertex& tail() const {
+    if (orientation) {
+      return *t;
+    } else {
+      return *s;
+    }
+  }
+  const Vertex& head() const {
+    if (orientation) {
+      return *s;
+    } else {
+      return *t;
+    }
+  }
+  bool isTight() const {
+    return std::abs(s->y + t->y - weight) < 1e-10;
+  }
+};
+
+bool Vertex::inMatching() const {
+  for (const Edge& e : neighbors) {
+    if (e.orientation) {
+      return true;
+    }
+  }
+  return false;
+}
+
+class Graph {
+public:
+  std::vector<Vertex> S;
+  std::vector<Vertex> T;
+  std::vector<Edge> E;
+
+  Graph(unsigned n, Rng& r) : S(n * (n + 1)), T(n * (n + 1)), E(pow(2 * n, 2)) {
+    for (unsigned i = 0; i < S.size(); i++) {
+      S[i].index = i;
+      S[i].coordinate = {2 * (i % (n + 1)), 2 * (i / (n + 1)) + 1};
+      T[i].index = S.size() + i;
+      T[i].coordinate = {2 * (i % n) + 1, 2 * (i / n)};
+    }
+    for (unsigned i = 0; i < E.size(); i++) {
+      Vertex& redVertex = S[(1 + (i % (2 * n))) / 2 + (n + 1) * ((i / 4) / n)];
+      Vertex& blueVertex = T[(i % (2 * n)) / 2 + n * (((i + 2 * n) / 4) / n)];
+      E[i].setVertices(redVertex, blueVertex);
+      E[i].weight = r.variate<double, std::exponential_distribution>(1);
+    }
+  }
+
+  void hungarian() {
+    std::queue<std::reference_wrapper<Vertex>> q;
+
+    for (Vertex &v : T) {
+      v.inZ = false;
+    }
+
+    for (Vertex &v : S) {
+      v.inZ = false;
+
+      if (!v.inMatching()) {
+        q.push(v);
+      }
+    }
+
+    while (!q.empty()) {
+      Vertex& v = q.front();
+      q.pop();
+
+      v.inZ = true;
+
+      for (Edge& e : v.neighbors) {
+        if (e.isTight() && (e.tail().index == v.index && !e.head().inZ)) {
+          q.push(e.head());
+        }
+      }
+    }
+
+    bool reversedPath = false;
+
+    for (Vertex& v : T) {
+      if (v.inZ && !v.inMatching()) {
+        std::stack<std::tuple<std::vector<std::reference_wrapper<Edge>>::iterator, std::vector<std::reference_wrapper<Edge>>::iterator, unsigned>> path;
+
+        for (Edge& e : E) {
+          e.inPath = false;
+        }
+
+        path.push({v.neighbors.begin(), v.neighbors.end(), v.index});
+
+        while (!path.empty()) {
+          auto [it, itEnd, i] = path.top();
+          if (it == itEnd) {
+            path.pop();
+            if (path.empty()) {
+              exit(1);
+            }
+            std::tie(it, itEnd, i) = path.top();
+            it->get().inPath = false;
+            it++;
+            path.top() = {it, itEnd, i};
+          } else {
+            Edge& e = it->get();
+
+            if (e.isTight() && (e.head().index == i && !e.inPath)) {
+              if (!e.tail().inMatching()) {
+                break;
+              }
+
+              e.inPath= true;
+              path.push({e.tail().neighbors.begin(), e.tail().neighbors.end(), e.tail().index});
+            } else {
+              it++;
+              path.top() = {it, itEnd, i};
+            }
+          }
+        }
+
+
+        if (path.empty()) {
+          exit(1);
+        }
+
+
+        while (!path.empty()) {
+          auto [it, itEnd, i] = path.top();
+          Edge& e = it->get();
+          path.pop();
+          e.orientation = !e.orientation;
+        }
+
+        reversedPath = true;
+        break;
+      }
+    }
+
+    if (!reversedPath) {
+      double Δ = std::numeric_limits<double>::infinity();
+
+      for (Edge& e : E) {
+        if (e.s->inZ && !e.t->inZ) {
+          Δ = std::min(Δ, e.weight - e.s->y - e.t->y);
+        }
+      }
+
+      for (Vertex& v : S) {
+        if (v.inZ) {
+          v.y += Δ;
+        }
+      }
+
+      for (Vertex& v : T) {
+        if (v.inZ) {
+          v.y -= Δ;
+        }
+      }
+
+    }
+  }
+
+  bool isPerfect() const {
+    for (const Vertex& v : S) {
+      if (!v.inMatching()) {
+        return  false;
+      }
+    }
+    for (const Vertex& v : T) {
+      if (!v.inMatching()) {
+        return  false;
+      }
+    }
+
+    return true;
+  }
+
+};
+
+int main(int argc, char* argv[]) {
+  unsigned n = 100;
+  unsigned maxSteps = 1e8;
+  double beliefThreshold = 1;
+
+  int opt;
+
+  while ((opt = getopt(argc, argv, "n:m:t:")) != -1) {
+    switch (opt) {
+    case 'n':
+      n = atoi(optarg);
+      break;
+    case 'm':
+      maxSteps = (unsigned)atof(optarg);
+      break;
+    case 't':
+      beliefThreshold = atof(optarg);
+      break;
+    default:
+      exit(1);
+    }
+  }
+
+  Rng r;
+  Graph G(n, r);
+
+  unsigned steps = 0;
+  double minBelief = 0;
+
+  while (!G.isPerfect()) {
+    G.hungarian();
+  }
+
+  for (const Edge& e : G.E) {
+    if (e.orientation) {
+      std::cout
+        << e.tail().coordinate[0] << " "
+        << e.tail().coordinate[1] << " "
+        << e.head().coordinate[0] << " "
+        << e.head().coordinate[1] << std::endl;
+    }
+  }
+
+  return 0;
+}
-- 
cgit v1.2.3-54-g00ecf