simon-tiger · April 8, 2022 05:00 · Aug 3, 2021 · Aug 2, 2021
diff --git a/pathfinding.js b/pathfinding.js
@@ -75,4 +75,5 @@ function shortestPath(start, goal, heuristic=() => 0) {
       }
     }
   }
+  return null;
 }
diff --git a/pathfinding.js b/pathfinding.js
@@ -0,0 +1,78 @@
+// Super simple pathfinding library
+
+class Node {
+  constructor(userData=null) {
+    this.connected = [];
+    this.userData = userData;
+  }
+
+  connect(node, weight=1, oneWay=false) {
+    this.connected.push({ node, weight });
+    if (!oneWay) {
+      node.connected.push({ node: this, weight });
+    }
+  }
+}
+
+const sqrt2 = Math.sqrt(2);
+const Distance = {
+  euclidean(x1, y1, x2, y2) {
+    return Math.hypot(x2-x1, y2-y1);
+  },
+  taxicab(x1, y1, x2, y2) {
+    return Math.abs(x2-x1) + Math.abs(y2-y1);
+  },
+  maximum(x1, y1, x2, y2) { // Ik, this isn't really the maximum metric, but I couldn't come up with a better name so yeah
+    const dx = Math.abs(x2-x1);
+    const dy = Math.abs(y2-y1);
+    if (dx > dy) {
+      return sqrt2*dy + dx-dy;
+    } else {
+      return sqrt2*dx + dy-dx;
+    }
+  }
+}
+
+// Use A* to find the shortest path between two nodes
+// If no heuristic is specified, it uses Dijkstra instead
+function shortestPath(start, goal, heuristic=() => 0) {
+  const openSet = new Set();
+  const steps = new Map();
+  const f = new Map();
+  const g = new Map();
+  openSet.add(start);
+  g.set(start, 0);
+  f.set(start, heuristic(start, goal));
+  while (openSet.size > 0) {
+    let current = null;
+    let recordF = Infinity;
+    for (const node of openSet) {
+      const fScore = f.get(node);
+      const h = heuristic(start, node);
+      const recordH = heuristic(start, current);
+      if (fScore < recordF || fScore === recordF && h < recordH) {
+        recordF = fScore;
+        current = node;
+      }
+    }
+    if (current === goal) {
+      let prev = current;
+      const path = [prev];
+      while (prev !== start) {
+        prev = steps.get(prev);
+        path.push(prev);
+      }
+      return path.reverse();
+    }
+    openSet.delete(current);
+    for (const { node, weight } of current.connected) {
+      const newG = g.get(current) + weight;
+      if (!g.has(node) || newG < g.get(node)) {
+        g.set(node, newG);
+        f.set(node, newG + heuristic(start, node));
+        steps.set(node, current);
+        openSet.add(node);
+      }
+    }
+  }
+}