@@ -348,7 +348,7 @@ pub fn shortest_path(
348348 seen : dict . new ( ) ,
349349 todos : priority_queue . from_list (
350350 [ ShortestPathTodo ( origin , [ ] , 0 ) ] ,
351- compare_todo ,
351+ compare_shortest_path_todo ,
352352 ) ,
353353 neighbours : neighbours ,
354354 )
@@ -402,7 +402,7 @@ fn shortest_path_aux(
402402 }
403403}
404404
405- fn compare_todo ( a : ShortestPathTodo , b : ShortestPathTodo ) {
405+ fn compare_shortest_path_todo ( a : ShortestPathTodo , b : ShortestPathTodo ) {
406406 int . compare ( a . steps_count , b . steps_count )
407407}
408408
@@ -691,3 +691,69 @@ pub fn dims_of_size(width width: Int, height height: Int) -> Dims {
691691 max_y : height - 1 ,
692692 )
693693}
694+
695+ pub fn manhattan_distance ( a : XY , b : XY ) -> Int {
696+ int . absolute_value ( a . 0 - b . 0 ) + int . absolute_value ( a . 1 - b . 1 )
697+ }
698+
699+ pub type AstarTodo {
700+ AstarTodo ( xy : XY , cost : Int )
701+ }
702+
703+ fn compare_astar_todo ( a : AstarTodo , b : AstarTodo ) {
704+ int . compare ( a . cost , b . cost )
705+ }
706+
707+ pub fn astar (
708+ from from : XY ,
709+ to to : XY ,
710+ cost cost : fn ( XY , XY ) -> Int ,
711+ neighbours neighbours : fn ( XY ) -> List ( XY ) ,
712+ print_progress print_progress : fn ( XY , List ( XY ) , Queue ( AstarTodo ) ) -> Nil ,
713+ ) -> Result ( Int , Nil ) {
714+ let frontier =
715+ priority_queue . new ( compare_astar_todo )
716+ |> priority_queue . push ( AstarTodo ( from , 0 ) )
717+ let cost_so_far : Dict ( XY , Int ) = dict . from_list ( [ # ( from , 0 ) ] )
718+ astar_aux ( to , cost , neighbours , frontier , cost_so_far , print_progress )
719+ }
720+
721+ fn astar_aux (
722+ to goal : XY ,
723+ cost cost : fn ( XY , XY ) -> Int ,
724+ neighbours neighbours : fn ( XY ) -> List ( XY ) ,
725+ frontier frontier : Queue ( AstarTodo ) ,
726+ cost_so_far cost_so_far : Dict ( XY , Int ) ,
727+ print_progress print_progress : fn ( XY , List ( XY ) , Queue ( AstarTodo ) ) -> Nil ,
728+ ) -> Result ( Int , Nil ) {
729+ use # ( current , rest ) <- result . try ( priority_queue . pop ( frontier ) )
730+ use <- bool . guard ( when : current . xy == goal , return : Ok ( current . cost ) )
731+ let nexts = neighbours ( current . xy )
732+ print_progress ( current . xy , nexts , frontier )
733+ let # ( new_frontier , new_cost_so_far ) =
734+ list . fold ( over : nexts , from : # ( rest , cost_so_far ) , with : fn ( acc , next ) {
735+ let # ( acc_frontier , acc_cost_so_far ) = acc
736+ let assert Ok ( current_cost ) = dict . get ( acc_cost_so_far , current . xy )
737+ let new_next_cost = current_cost + cost ( current . xy , next )
738+ case dict . get ( acc_cost_so_far , next ) {
739+ Ok ( old_next_cost ) if old_next_cost <= new_next_cost -> acc
740+ _ -> {
741+ let new_cost_so_far =
742+ dict . insert ( acc_cost_so_far , next , new_next_cost )
743+ let priority = new_next_cost + manhattan_distance ( next , goal )
744+ let new_frontier =
745+ acc_frontier
746+ |> priority_queue . push ( AstarTodo ( xy : next , cost : priority ) )
747+ # ( new_frontier , new_cost_so_far )
748+ }
749+ }
750+ } )
751+ astar_aux (
752+ goal ,
753+ cost ,
754+ neighbours ,
755+ new_frontier ,
756+ new_cost_so_far ,
757+ print_progress ,
758+ )
759+ }
0 commit comments