1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
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>
|
