-
-
Notifications
You must be signed in to change notification settings - Fork 11
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor: migrated astar to use a priorityqueue
- Loading branch information
Showing
7 changed files
with
173 additions
and
105 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
49 changes: 49 additions & 0 deletions
49
solutions/typescript/libs/lib/src/pathfinding/astar.spec.ts
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| /* eslint-disable @typescript-eslint/no-non-null-assertion */ | ||
| import { describe, expect, it } from 'vitest'; | ||
| import { Direction } from '../model/direction/direction.class.js'; | ||
| import { GridGraph } from '../model/graph/grid-graph.class.js'; | ||
| import { Vec2 } from '../model/vector/vec2.class.js'; | ||
| import { aStar } from './astar.js'; | ||
|
|
||
| describe('A Star', () => { | ||
| const matrix = [ | ||
| ['#', '#', '#', '#', '#', '#', '#'], | ||
| ['#', '.', '.', '#', '.', '.', '#'], | ||
| ['#', '.', '#', '.', '.', '.', '#'], | ||
| ['#', '.', '.', '.', '#', '.', '#'], | ||
| ['#', '.', '.', '.', '#', '.', '#'], | ||
| ['#', '#', '#', '#', '#', '#', '#'], | ||
| ]; | ||
| const start = new Vec2(1, 1); | ||
| const finish = new Vec2(5, 4); | ||
|
|
||
| it('should be able to generate a graph from a matrix', () => { | ||
| const g = GridGraph.fromMatrix(matrix); | ||
| expect([...g.nodes.values()].length).toEqual(matrix.length * (matrix[0]?.length ?? 0)); | ||
| }); | ||
|
|
||
| it('should find the shortest path', () => { | ||
| const g = GridGraph.fromMatrix(matrix); | ||
| const goal = g.getNode(finish)!; | ||
| const { path } = g.aStar({ | ||
| start: g.getNode(start)!, | ||
| end: goal, | ||
| heuristic: (a) => a.coordinate.manhattan(goal.coordinate), | ||
| }); | ||
| expect(path.length).toEqual(10); | ||
| }); | ||
|
|
||
| it('should find the shortest path with diagonal connections', () => { | ||
| const g = GridGraph.fromMatrix(matrix, { | ||
| connectionDirections: Direction.allDirections, | ||
| }); | ||
| const goal = g.getNode(finish)!; | ||
| const { path } = aStar({ | ||
| allNodes: g.nodes, | ||
| start: g.getNode(start)!, | ||
| end: goal, | ||
| heuristic: (a) => a.coordinate.manhattan(goal.coordinate), | ||
| }); | ||
| expect(path.length).toEqual(6); | ||
| }); | ||
| }); |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,12 +1,112 @@ | ||
| import { isNotNullish } from '@alexaegis/common'; | ||
| import { PriorityQueue } from 'js-sdsl'; | ||
| import type { GraphTraversalOptions } from '../model/graph/graph.class.js'; | ||
| import type { BasicGraphNode } from '../model/graph/node.class.js'; | ||
| import type { ToString } from '../model/to-string.interface.js'; | ||
| import type { EdgeCollectionOptions, PathFindingResult } from './dijkstra.js'; | ||
| import { | ||
| calculateCostOfTraversal, | ||
| collectEdges, | ||
| constructPath, | ||
| type EdgeCollectionOptions, | ||
| type PathConstructor, | ||
| type PathFindingResult, | ||
| } from './dijkstra.js'; | ||
|
|
||
| export const aStar = <T extends ToString, Dir extends ToString, N extends BasicGraphNode<T, Dir>>( | ||
| options: GraphTraversalOptions<T, Dir, N> & | ||
| Omit<EdgeCollectionOptions<T, Dir, N>, 'pathConstructor'>, | ||
| ): PathFindingResult<N> => { | ||
| console.log(options); | ||
| return { distances: new Map(), path: [] }; | ||
| if (!options.start || !options.end) { | ||
| return { path: [], distances: new Map() }; | ||
| } | ||
|
|
||
| const h = options?.heuristic ?? (() => 1); | ||
|
|
||
| const prev = new Map<N, N>(); // prev! | ||
| const gScore = new Map<N, number>(); // dist! Infinity | ||
| const fScore = new Map<N, number>(); // Infinity | ||
| const pathLengthMap = new Map<N, number>(); // How many nodes there are to reach the end | ||
|
|
||
| gScore.set(options.start, 0); | ||
| fScore.set(options.start, h(options.start, [])); | ||
| pathLengthMap.set(options.start, 0); | ||
| // This is used to support weights of 0 | ||
| const orderOfDiscovery = [options.start]; | ||
|
|
||
| const pq = new PriorityQueue<N>([options.start], (a, b) => { | ||
| const aScore = fScore.get(a) ?? Number.POSITIVE_INFINITY; | ||
| const bScore = fScore.get(b) ?? Number.POSITIVE_INFINITY; | ||
|
|
||
| if (aScore === bScore) { | ||
| let aPathLength = orderOfDiscovery.indexOf(a); | ||
| let bPathLength = orderOfDiscovery.indexOf(b); | ||
| if (aPathLength < 0) { | ||
| aPathLength = Number.POSITIVE_INFINITY; | ||
| } | ||
| if (bPathLength < 0) { | ||
| bPathLength = Number.POSITIVE_INFINITY; | ||
| } | ||
| return aPathLength - bPathLength; | ||
| } else { | ||
| return aScore - bScore; | ||
| } | ||
| }); | ||
|
|
||
| const pathConstructor: PathConstructor<N> = (to) => constructPath<N>(options.start, to, prev); | ||
|
|
||
| const isFinished = isNotNullish(options.end) | ||
| ? (n: N) => | ||
| typeof options.end === 'function' | ||
| ? options.end(n, pathConstructor(n)) | ||
| : n === options.end | ||
| : (_n: N) => false; | ||
|
|
||
| let goal: N | undefined; | ||
|
|
||
| while (pq.length > 0) { | ||
| // eslint-disable-next-line @typescript-eslint/no-non-null-assertion | ||
| const current = pq.pop()!; // u, closest yet | ||
|
|
||
| orderOfDiscovery.removeItem(current); | ||
|
|
||
| if (isFinished(current)) { | ||
| goal = current; | ||
| break; | ||
| } | ||
|
|
||
| const uDist = gScore.get(current) ?? Number.POSITIVE_INFINITY; | ||
|
|
||
| for (const neighbour of collectEdges<T, Dir, N>(current, { | ||
| pathConstructor, | ||
| allNodes: options.allNodes, | ||
| edgeFilter: options.edgeFilter, | ||
| edgeGenerator: options.edgeGenerator, | ||
| })) { | ||
| const weight = calculateCostOfTraversal<T, Dir, N>( | ||
| neighbour, | ||
| options.currentPathWeighter, | ||
| pathConstructor, | ||
| ); | ||
| const tentativegScore = uDist + weight; | ||
| const currentScore = gScore.get(neighbour.to) ?? Number.POSITIVE_INFINITY; | ||
| if (tentativegScore < currentScore) { | ||
| const currentPath = constructPath<N>(options.start, current, prev); | ||
|
|
||
| prev.set(neighbour.to, current); | ||
| gScore.set(neighbour.to, tentativegScore); | ||
| fScore.set(neighbour.to, tentativegScore + h(neighbour.to, currentPath)); | ||
| pathLengthMap.set(neighbour.to, (pathLengthMap.get(neighbour.to) ?? 0) + 1); | ||
|
|
||
| if (!pq.updateItem(neighbour.to)) { | ||
| pq.push(neighbour.to); | ||
| orderOfDiscovery.push(neighbour.to); | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| return { | ||
| path: constructPath<N>(options.start, goal, prev), | ||
| distances: pathLengthMap, | ||
| }; | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters