From b227d5073b0fd2b3c14ce1d32c928881cefc5222 Mon Sep 17 00:00:00 2001
From: Jonathan Calvert <38069151+JDCalvert@users.noreply.github.com>
Date: Mon, 28 Feb 2022 17:23:46 +0000
Subject: [PATCH] Pathfinding Improvements: Replace nextNodes array with
 PriorityQueueSet (#169)

---
 js/data_structures.js | 82 +++++++++++++++++++++++++++++++++++++++++++
 js/pathfinding.js     | 42 +++++++++++++---------
 2 files changed, 107 insertions(+), 17 deletions(-)
 create mode 100644 js/data_structures.js

diff --git a/js/data_structures.js b/js/data_structures.js
new file mode 100644
index 0000000..4337b6d
--- /dev/null
+++ b/js/data_structures.js
@@ -0,0 +1,82 @@
+/**
+ * A combination queue/set where the elements are ordered (in ascending order, according to the given priority function)
+ * and unique (according to the given elementMatcher).
+ * 
+ * If an element is added to the set and an equivalent element already exists, the lower-priority one is discarded.
+ */
+export class PriorityQueueSet {
+	constructor(elementMatcher, priorityFunction) {
+		this.first = null;
+		this.elementMatcher = elementMatcher;
+		this.priorityFunction = priorityFunction;
+	}
+
+	pushWithPriority(value) {
+		const newNode = {value, priority: this.priorityFunction(value), next: null};
+
+		// If the queue is currently empty, we can just set this new node as the first and we're done
+		if (!this.first) {
+			this.first = newNode;
+			return;
+		}
+
+		let inserted = false;
+		let previous;
+		let current = this.first;
+
+		// Loop through the existing elements
+		while (current) {
+			if (this.elementMatcher(current.value, value)) {
+				// We've found an equivalent element before one with a lower priority. This one has at least
+				// the same priority as the new one, so don't bother inserting
+				return;
+			} else if (newNode.priority <= current.priority) {
+				// We've found some element with lower priority than the new one, so insert the new one just before it
+				newNode.next = current;
+				if (previous) {
+					previous.next = newNode;
+				} else {
+					this.first = newNode;
+				}
+				inserted = true;
+
+				previous = current;
+				current = current.next
+				break;
+			}
+			previous = current;
+			current = current.next;
+		}
+
+		if (inserted) {
+			// Go through the rest of the list and try to find an equivalent element to the new one.
+			// We know it has higher priority than the new one, so remove it.
+			while (current) {
+				if (this.elementMatcher(current.value, value)) {
+					if (previous) {
+						previous.next = current.next;
+					} else {
+						this.first = current.next
+					}
+					return;
+				}
+				previous = current;
+				current = current.next;
+			}
+		} else {
+			// We reached the end of the queue without finding a lower-priority or existing element, so
+			// insert the new one at the end
+			previous.next = newNode;
+		}
+	}
+
+	hasNext() {
+		return !!this.first;
+	}
+
+	pop() {
+		const first = this.first;
+		this.first = first?.next;
+		return first?.value;
+	}
+}
diff --git a/js/pathfinding.js b/js/pathfinding.js
index e80a291..2ff80a6 100644
--- a/js/pathfinding.js
+++ b/js/pathfinding.js
@@ -5,6 +5,7 @@ import {settingsKey} from "./settings.js";
 import {getSnapPointForTokenObj, iterPairs} from "./util.js";
 
 import * as GridlessPathfinding from "../wasm/gridless_pathfinding.js"
+import {PriorityQueueSet} from "./data_structures.js";
 
 let cachedNodes = undefined;
 let use5105 = false;
@@ -103,32 +104,39 @@ function calculatePath(from, to, token, previousWaypoints) {
 		previousWaypoints = previousWaypoints.map(w => getGridPositionFromPixelsObj(w));
 		startLayer = calcNoDiagonals(previousWaypoints) % 2;
 	}
-	const nextNodes = [{node: getNode({...to, layer: startLayer}, token), cost: 0, estimated: estimateCost(to, from), previous: null}];
+
+	const nextNodes = new PriorityQueueSet((node1, node2) => node1.node === node2.node, node => node.estimated);
 	const previousNodes = new Set();
-	while (nextNodes.length > 0) {
-		// Sort by estimated cost, high to low
-		// TODO Re-sorting every iteration is expensive. Think of something better
-		nextNodes.sort((a, b) => b.estimated - a.estimated);
+
+	nextNodes.pushWithPriority(
+		{
+			node: getNode({...to, layer: startLayer}, token),
+			cost: 0,
+			estimated: estimateCost(to, from),
+			previous: null
+		}
+	);
+
+	while (nextNodes.hasNext()) {
 		// Get node with cheapest estimate
 		const currentNode = nextNodes.pop();
-		if (currentNode.node.x === from.x && currentNode.node.y === from.y)
+		if (currentNode.node.x === from.x && currentNode.node.y === from.y) {
 			return currentNode;
+		}
 		previousNodes.add(currentNode.node);
 		for (const edge of currentNode.node.edges) {
 			const neighborNode = getNode(edge.target, token);
-			if (previousNodes.has(neighborNode))
+			if (previousNodes.has(neighborNode)) {
 				continue;
-			const neighbor = {node: neighborNode, cost: currentNode.cost + edge.cost, estimated: currentNode.cost + edge.cost + estimateCost(neighborNode, from), previous: currentNode};
-			const neighborIndex = nextNodes.findIndex(node => node.node === neighbor.node);
-			if (neighborIndex >= 0) {
-				// If the neighbor is cheaper to reach via the current route than through previously discovered routes, replace it
-				if (nextNodes[neighborIndex].cost > neighbor.cost) {
-					nextNodes[neighborIndex] = neighbor;
-				}
-			}
-			else {
-				nextNodes.push(neighbor);
 			}
+
+			const neighbor = {
+				node: neighborNode,
+				cost: currentNode.cost + edge.cost,
+				estimated: currentNode.cost + edge.cost + estimateCost(neighborNode, from),
+				previous: currentNode
+			};
+			nextNodes.pushWithPriority(neighbor);
 		}
 	}
 }