extra/path-finding/path-finding.factor

   1 ! Copyright (C) 2010 Samuel Tardieu.
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: accessors assocs heaps kernel math sequences sets ;
   4 IN: path-finding
   5
   6 ! This implements the A* algorithm. See http://en.wikipedia.org/wiki/A*
   7
   8 TUPLE: astar g in-closed-set ;
   9 GENERIC: cost ( from to astar -- n )
  10 GENERIC: heuristic ( from to astar -- n )
  11 GENERIC: neighbors ( node astar -- seq )
  12
  13 <PRIVATE
  14
  15 TUPLE: (astar) astar goal origin in-open-set open-set ;
  16
  17 : (add-to-open-set) ( h node astar -- )
  18     2dup in-open-set>> at* [ over open-set>> heap-delete ] [ drop ] if
  19     [ swapd open-set>> heap-push* ] [ in-open-set>> set-at ] 2bi ;
  20
  21 : add-to-open-set ( node astar -- )
  22     [ astar>> g>> at ] 2keep
  23     [ [ goal>> ] [ astar>> heuristic ] bi + ] 2keep
  24     (add-to-open-set) ;
  25
  26 : ?add-to-open-set ( node astar -- )
  27     2dup astar>> in-closed-set>> in? [ 2drop ] [ add-to-open-set ] if ;
  28
  29 : move-to-closed-set ( node astar -- )
  30     [ astar>> in-closed-set>> adjoin ] [ in-open-set>> delete-at ] 2bi ;
  31
  32 : get-first ( astar -- node )
  33     [ open-set>> heap-pop drop dup ] [ move-to-closed-set ] bi ;
  34
  35 : set-g ( origin g node astar -- )
  36     [ [ origin>> set-at ] [ astar>> g>> set-at ] bi-curry bi-curry bi* ] [ ?add-to-open-set ] 2bi ;
  37
  38 : cost-through ( origin node astar -- cost )
  39     [ astar>> cost ] [ nip astar>> g>> at ] 3bi + ;
  40
  41 : ?set-g ( origin node astar -- )
  42     [ cost-through ] 3keep [ swap ] 2dip
  43     3dup astar>> g>> at [ 1/0. ] unless* >= [ 4drop ] [ set-g ] if ;
  44
  45 : build-path ( target astar -- path )
  46     [ over ] [ over [ [ origin>> at ] keep ] dip ] produce 2nip reverse ;
  47
  48 : handle ( node astar -- )
  49     dupd [ astar>> neighbors ] keep [ ?set-g ] curry with each ;
  50
  51 : (find-path) ( astar -- path/f )
  52     dup open-set>> heap-empty? [
  53         drop f
  54     ] [
  55         [ get-first ] keep 2dup goal>> = [ build-path ] [ [ handle ] [ (find-path) ] bi ] if
  56     ] if ;
  57
  58 : (init) ( from to astar -- )
  59     swap >>goal
  60     H{ } clone over astar>> g<<
  61     HS{ } clone over astar>> in-closed-set<<
  62     H{ } clone >>origin
  63     H{ } clone >>in-open-set
  64     <min-heap> >>open-set
  65     [ 0 ] 2dip [ (add-to-open-set) ] [ astar>> g>> set-at ] 3bi ;
  66
  67 TUPLE: astar-simple < astar cost heuristic neighbors ;
  68 M: astar-simple cost cost>> call( n1 n2 -- c ) ;
  69 M: astar-simple heuristic heuristic>> call( n1 n2 -- c ) ;
  70 M: astar-simple neighbors neighbors>> call( n -- neighbors ) ;
  71
  72 TUPLE: bfs < astar neighbors ;
  73 M: bfs cost 3drop 1 ;
  74 M: bfs heuristic 3drop 0 ;
  75 M: bfs neighbors neighbors>> at ;
  76
  77 TUPLE: dijkstra < astar costs ;
  78 M: dijkstra cost costs>> swapd at at ;
  79 M: dijkstra heuristic 3drop 0 ;
  80 M: dijkstra neighbors costs>> at keys ;
  81
  82 PRIVATE>
  83
  84 : find-path ( start target astar -- path/f )
  85     (astar) new [ astar<< ] keep [ (init) ] [ (find-path) ] bi ;
  86
  87 : <astar> ( neighbors cost heuristic -- astar )
  88     astar-simple new swap >>heuristic swap >>cost swap >>neighbors ;
  89
  90 : considered ( astar -- considered )
  91     in-closed-set>> members ;
  92
  93 : <bfs> ( neighbors -- astar )
  94     [ bfs new ] dip >>neighbors ;
  95
  96 : <dijkstra> ( costs -- astar )
  97     [ dijkstra new ] dip >>costs ;