Add License and ReadmeHEADmaster

Add pathfinder files
Change start to be simpler

1 files changed, 109 insertions, 0 deletions

diff --git a/src/pathfinder/index.ts b/src/pathfinder/index.ts
new file mode 100644
index 0000000..dc11e2e
--- /dev/null
+++ b/src/pathfinder/index.ts
@@ -0,0 +1,109 @@
+/**
+  * @module Pathfinder
+*/
+
+import { Graph, Vertex } from "./Graph";
+
+/**
+  * Function that find path from start to goal in given map
+  * @param map - map
+*/
+export default function findPath(map: Graph): Path | null {
+  const start = map.find(v => v.start)
+  const goal = map.find(v => v.goal)
+
+  if (!start || !goal) {
+    throw new Error("Pathfinder@findPath: There's no start and/or goal in given graph")
+  }
+
+  return A_Star(start, () => ((start.x! - goal.x!) ** 2 + (start.y! - goal.y!) ** 2) * 0.5);
+}
+
+/**
+  * Function implementing A Star algorith
+  @param start - start
+  @param h - heuristic function
+*/
+function A_Star(start: Vertex, h: (arg0: Vertex) => number): Path | null {
+  type verObj = { [index: string]: Vertex }
+  type numObj = { [index: string]: number }
+  const openSet = [start]
+  const cameFrom: verObj = {}
+  const gScore: numObj = {}
+  gScore[start.uuid] = 0
+  const fScore: numObj = {}
+  fScore[start.uuid] = h(start)
+
+  while (openSet.length) {
+    const current = smallest(openSet, (pv, cv) => getOrInf(fScore, pv.uuid) < getOrInf(fScore, cv.uuid))
+    if (current.goal) return reconstruct_path(cameFrom, current)
+    removeItem(openSet, current)
+
+    const neighbours = current.edges.filter(edge => edge.to.walkable).map(edge => { return { edge, neighbour: edge.to } });
+    for (const { neighbour, edge } of neighbours) {
+      const tentative_gScore = getOrInf(gScore, current.uuid) + edge.weight
+      if (tentative_gScore < getOrInf(gScore, neighbour.uuid)) {
+        cameFrom[neighbour.uuid] = current
+        gScore[neighbour.uuid] = tentative_gScore
+        fScore[neighbour.uuid] = tentative_gScore + h(neighbour)
+        if (!openSet.includes(neighbour)) openSet.push(neighbour)
+      }
+    }
+  }
+  return null;
+}
+
+/**
+  * Function that reconstructs path, from finish to start
+  * @param cameFrom - finish
+  * @param current - whereami vertex
+*/
+function reconstruct_path(cameFrom: { [index: string]: Vertex }, current: Vertex): Path {
+  let c = current;
+  const total_path = [c]
+  while (Object.keys(cameFrom).includes(c.uuid)) {
+    c = cameFrom[c.uuid];
+    total_path.unshift(c)
+  }
+  return total_path;
+}
+
+/** 
+  * Getter to get value or object or null
+  * @param arr - object to look in
+  * @param key - key at which to search
+*/
+function getOrInf(arr: { [index: string]: number }, key: keyof typeof arr): number {
+  return arr[key] ?? Infinity
+}
+
+/**
+  * Gets smallest element from array 
+  * @typeParam T - typeof elements inside given array
+  * @param arr - array to look in 
+  * @param fn - function to determine which is smaller element
+*/
+function smallest<T>(arr: Array<T>, fn: (prev: T, curr: T) => boolean): T {
+  let smallest = arr[0]
+  arr.forEach((el, i) => {
+    if (!i) return;
+    if (fn(arr[i - 1], el)) smallest = arr[i - 1]
+  });
+  return smallest;
+}
+
+/**
+  * Remove element from given array
+  * @typeParam T - typeof elements inside given array
+  * @param item - to be removed
+*/
+function removeItem<T>(arr: Array<T>, item: T): Array<T> {
+  const index = arr.indexOf(item);
+  if (index > -1) {
+    arr.splice(index, 1);
+  }
+  return arr;
+}
+
+/** Shortcut */
+export type Path = Array<Vertex>