factor/extra/path-finding/path-finding.factor

98 lines
3.0 KiB
Factor
Raw Normal View History

2010-03-06 02:48:39 -05:00
! Copyright (C) 2010 Samuel Tardieu.
! See http://factorcode.org/license.txt for BSD license.
2010-03-17 05:50:45 -04:00
USING: accessors assocs hash-sets heaps kernel math sequences sets shuffle ;
2010-03-23 04:52:51 -04:00
IN: path-finding
2010-03-06 02:48:39 -05:00
! This implements the A* algorithm. See http://en.wikipedia.org/wiki/A*
TUPLE: astar g in-closed-set ;
GENERIC: cost ( from to astar -- n )
GENERIC: heuristic ( from to astar -- n )
GENERIC: neighbours ( node astar -- seq )
2010-03-06 02:48:39 -05:00
<PRIVATE
TUPLE: (astar) astar goal origin in-open-set open-set ;
2010-03-06 02:48:39 -05:00
: (add-to-open-set) ( h node astar -- )
2dup in-open-set>> at* [ over open-set>> heap-delete ] [ drop ] if
[ swapd open-set>> heap-push* ] [ in-open-set>> set-at ] 2bi ;
: add-to-open-set ( node astar -- )
[ astar>> g>> at ] 2keep
[ [ goal>> ] [ astar>> heuristic ] bi + ] 2keep
2010-03-06 02:48:39 -05:00
(add-to-open-set) ;
: ?add-to-open-set ( node astar -- )
2010-03-17 05:50:45 -04:00
2dup astar>> in-closed-set>> in? [ 2drop ] [ add-to-open-set ] if ;
2010-03-06 02:48:39 -05:00
: move-to-closed-set ( node astar -- )
2010-03-17 05:50:45 -04:00
[ astar>> in-closed-set>> adjoin ] [ in-open-set>> delete-at ] 2bi ;
2010-03-06 02:48:39 -05:00
: get-first ( astar -- node )
[ open-set>> heap-pop drop dup ] [ move-to-closed-set ] bi ;
: set-g ( origin g node astar -- )
[ [ origin>> set-at ] [ astar>> g>> set-at ] bi-curry bi-curry bi* ] [ ?add-to-open-set ] 2bi ;
2010-03-06 02:48:39 -05:00
: cost-through ( origin node astar -- cost )
[ astar>> cost ] [ nip astar>> g>> at ] 3bi + ;
2010-03-06 02:48:39 -05:00
: ?set-g ( origin node astar -- )
[ cost-through ] 3keep [ swap ] 2dip
3dup astar>> g>> at [ 1/0. ] unless* >= [ 4drop ] [ set-g ] if ;
2010-03-06 02:48:39 -05:00
: build-path ( target astar -- path )
[ over ] [ over [ [ origin>> at ] keep ] dip ] produce 2nip reverse ;
: handle ( node astar -- )
dupd [ astar>> neighbours ] keep [ ?set-g ] curry with each ;
2010-03-06 02:48:39 -05:00
: (find-path) ( astar -- path/f )
dup open-set>> heap-empty? [
drop f
] [
[ get-first ] keep 2dup goal>> = [ build-path ] [ [ handle ] [ (find-path) ] bi ] if
] if ;
: (init) ( from to astar -- )
swap >>goal
H{ } clone over astar>> g<<
HS{ } clone over astar>> in-closed-set<<
2010-03-06 02:48:39 -05:00
H{ } clone >>origin
H{ } clone >>in-open-set
<min-heap> >>open-set
[ 0 ] 2dip [ (add-to-open-set) ] [ astar>> g>> set-at ] 3bi ;
TUPLE: astar-simple < astar cost heuristic neighbours ;
M: astar-simple cost cost>> call( n1 n2 -- c ) ;
M: astar-simple heuristic heuristic>> call( n1 n2 -- c ) ;
M: astar-simple neighbours neighbours>> call( n -- neighbours ) ;
2010-03-06 02:48:39 -05:00
2010-03-23 05:24:01 -04:00
TUPLE: bfs < astar neighbours ;
M: bfs cost 3drop 1 ;
M: bfs heuristic 3drop 0 ;
M: bfs neighbours neighbours>> at ;
TUPLE: dijkstra < astar costs ;
M: dijkstra cost costs>> swapd at at ;
M: dijkstra heuristic 3drop 0 ;
M: dijkstra neighbours costs>> at keys ;
2010-03-06 02:48:39 -05:00
PRIVATE>
: find-path ( start target astar -- path/f )
(astar) new [ astar<< ] keep [ (init) ] [ (find-path) ] bi ;
2010-03-06 02:48:39 -05:00
: <astar> ( neighbours cost heuristic -- astar )
astar-simple new swap >>heuristic swap >>cost swap >>neighbours ;
2010-03-06 02:48:39 -05:00
: considered ( astar -- considered )
2010-03-17 05:50:45 -04:00
in-closed-set>> members ;
2010-03-23 05:24:01 -04:00
: <bfs> ( neighbours -- astar )
[ bfs new ] dip >>neighbours ;
: <dijkstra> ( costs -- astar )
[ dijkstra new ] dip >>costs ;