export function path_find(navmesh: NavMesh, origin: number, goal: number) {
let predecessors: VectorField = [];
let g: Array<number> = [];
g[origin] = 0;
let h: Array<number> = [];
h[origin] = 0;
let f: Array<number> = [];
f[origin] = 0;
let boundary = [origin];
while (boundary.length > 0) {
let lowest = lowest_cost(boundary, f);
let current = boundary.splice(lowest, 1)[0];
if (current === goal) {
return [...path_follow(predecessors, goal)];
}
for (let i = 0; i < navmesh.Graph[current].length; i++) {
let next = navmesh.Graph[current][i][0];
let cost = navmesh.Graph[current][i][1];
let g_next = g[current] + cost;
if (g[next] === undefined) {
h[next] = vec3_distance_squared(navmesh.Centroids[next], navmesh.Centroids[goal]);
g[next] = g_next;
f[next] = g_next + h[next];
predecessors[next] = current;
boundary.push(next);
} else if (g_next + EPSILON < g[next]) {
g[next] = g_next;
f[next] = g_next + h[next];
predecessors[next] = current;
}
}
}
return false;
}