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-70-g09d2