import { distance , squaredDistance } from '../util' ; import { RoadGraph , roadGraph } from './roadGraph' ; export function findPath ( start , end ) { const result = [ start ] ; const closestToStart = findNearestNode ( roadGraph , start ) ; const closestToEnd = findNearestNode ( roadGraph , end ) ; const chain = aStar ( roadGraph , closestToStart , closestToEnd , ( g , s ) => { const node = g [ s ] ; return node ? distance ( node . position , end ) : Infinity ; } ) ; for ( const i of chain ) { result . push ( roadGraph [ i ] ! . position ) ; } result . push ( end ) ; return result ; } function findNearestNode ( graph , pos ) { let distance = Infinity ; let closest = - 1 ; for ( let i = 0 ; i < graph . max ; i ++ ) { const node = graph [ i ] ; if ( ! node ) { continue ; } const candidateDistance = squaredDistance ( pos , node . position ) ; if ( candidateDistance < distance ) { distance = candidateDistance ; closest = i ; } } return closest ; } function reconstructPath ( cameFrom , current ) { let cur = current ; let totalPath = [ cur ] ; while ( true ) { cur = cameFrom . get ( cur ) ; if ( ! cur ) { return totalPath ; } totalPath = [ cur , ... totalPath ] ; } } function aStar ( graph , start , goal , h ) { const openSet = new Set < number > ( ) ; openSet . add ( start ) ; const cameFrom = new Map < number , number > ( ) ; const gScore = new Map < number , number > ( ) ; const fScore = new Map < number , number > ( ) ; for ( let i = 0 ; i < graph . max ; i ++ ) { gScore . set ( i , Infinity ) ; fScore . set ( i , Infinity ) ; } gScore . set ( start , 0 ) ; fScore . set ( start , h ( graph , start ) ) ; let current = start ; while ( openSet . size > 0 ) { current = findLowestFScore ( openSet , fScore ) ; if ( current === goal ) { return reconstructPath ( cameFrom , current ) ; } openSet . delete ( current ) ; const curNode = graph [ current ] ; if ( ! curNode ) { continue ; } for ( const neighbor of curNode . neighbors ) { const nNode = graph [ neighbor ] ; if ( ! nNode ) { continue ; } const tentativeGScore = gScore . get ( current ) ! + distance ( curNode . position , nNode . position ) ; if ( tentativeGScore < gScore . get ( neighbor ) ! ) { cameFrom . set ( neighbor , current ) ; gScore . set ( neighbor , tentativeGScore ) ; fScore . set ( neighbor , tentativeGScore + h ( graph , neighbor ) ) ; openSet . add ( neighbor ) ; } } } return [ ] ; } function findLowestFScore ( openSet , fScore ) { let score = Infinity ; let lowest = - 1 ; for ( const index of openSet ) { const candidateScore = fScore . get ( index ) ; if ( candidateScore ! < score ) { score = candidateScore ! ; lowest = index ; } } return lowest ; }