diff --git a/extra/astar/astar-docs.factor b/extra/astar/astar-docs.factor new file mode 100644 index 0000000000..b8da237ed6 --- /dev/null +++ b/extra/astar/astar-docs.factor @@ -0,0 +1,42 @@ +! Copyright (C) 2010 Samuel Tardieu. +! See http://factorcode.org/license.txt for BSD license. +USING: help.markup help.syntax ; +IN: astar + +{ find-path considered } related-words + +HELP: +{ $values + { "neighbours" "a quotation with stack effect ( node -- seq )" } + { "cost" "a quotation with stack effect ( from to -- cost )" } + { "heuristic" "a quotation with stack effect ( pos target -- cost )" } + { "astar" "a astar tuple" } +} +{ $description "Build an astar object from the given quotations. The " + { $snippet "neighbours" } " one builds the list of neighbours. The " + { $snippet "cost" } " and " { $snippet "heuristic" } " ones represent " + "respectively the cost for transitioning from a node to one of its neighbour, " + "and the underestimated cost for going from a node to the target." +} ; + +HELP: find-path +{ $values + { "start" "a node" } + { "target" "a node" } + { "astar" "a astar tuple" } + { "path/f" "an optimal path from " { $snippet "start" } " to " { $snippet "target" } + ", or f if no such path exists" } +} +{ $description "Find a path between " { $snippet "start" } " and " { $snippet "target" } + " using the A* algorithm. The " { $snippet "astar" } " tuple must have been previously " + " built using " { $link } "." +} ; + +HELP: considered +{ $values + { "astar" "a astar tuple" } + { "considered" "a sequence" } +} +{ $description "When called after a call to " { $link find-path } ", return a list of nodes " + "which have been examined during the A* exploration." +} ; diff --git a/extra/astar/astar-tests.factor b/extra/astar/astar-tests.factor new file mode 100644 index 0000000000..11b2dfcaa2 --- /dev/null +++ b/extra/astar/astar-tests.factor @@ -0,0 +1,109 @@ +! Copyright (C) 2010 Samuel Tardieu. +! See http://factorcode.org/license.txt for BSD license. +USING: arrays assocs astar combinators hashtables kernel literals math math.functions +math.vectors sequences sorting splitting strings tools.test ; +IN: astar.tests + +<< + +! Use a 10x9 maze (see below) to try to go from s to e, f or g. +! X means that a position is unreachable. +! The costs model is: +! - going up costs 5 points +! - going down costs 1 point +! - going left or right costs 2 points + +: reachable? ( pos -- ? ) + first2 [ 2 * 5 + ] [ 2 + ] bi* $[ +" 0 1 2 3 4 5 6 7 8 9 + + 0 X X X X X X X X X X + 1 X s f X X + 2 X X X X X X X X X + 3 X X X X X X X X X + 4 X X X X X X + 5 X X X X X + 6 X X X X X X e X + 7 X g X X + 8 X X X X X X X X X X" + "\n" split ] nth nth CHAR: X = not ; + +: neighbours ( pos -- neighbours ) + first2 + { [ 1 + 2array ] [ 1 - 2array ] [ [ 1 + ] dip 2array ] [ [ 1 - ] dip 2array ] } 2cleave + 4array + [ reachable? ] filter ; + +: heuristic ( from to -- cost ) + v- [ abs ] [ + ] map-reduce ; + +: cost ( from to -- cost ) + 2dup [ first ] bi@ = [ [ second ] bi@ > 1 5 ? ] [ 2drop 2 ] if ; + +: test1 ( to -- path considered ) + { 1 1 } swap [ neighbours ] [ cost ] [ heuristic ] [ find-path ] [ considered ] bi ; +>> + +! Existing path from s to f +[ + { + { 1 1 } + { 2 1 } + { 3 1 } + { 4 1 } + { 4 2 } + { 4 3 } + { 4 4 } + { 4 5 } + { 4 6 } + { 4 7 } + { 5 7 } + { 6 7 } + { 7 7 } + { 8 7 } + { 8 6 } + } +] [ + { 8 6 } test1 drop +] unit-test + +! Check that only the right positions have been considered in the s to f path +[ 7 ] [ { 7 1 } test1 nip length ] unit-test + +! Non-existing path from s to g -- all positions must have been considered +[ f 26 ] [ { 1 7 } test1 length ] unit-test + +<< + +! Look for a path between A and C. The best path is A --> D --> C. C will be placed +! in the open set early because B will be examined first. This checks that the evaluation +! of C is correctly replaced in the open set. +! +! We use no heuristic here and always return 0. +! +! (5) +! B ---> C <-------- +! \ (2) +! ^ ^ | +! | | | +! (1) | | (2) | +! | | | +! +! A ---> D ---------> E ---> F +! (2) (1) (1) + +: n ( pos -- neighbours ) + $[ { "ABD" "BC" "C" "DCE" "ECF" } [ unclip swap 2array ] map >hashtable ] at ; + +: c ( from to -- cost ) + "" 2sequence H{ { "AB" 1 } { "AD" 2 } { "BC" 5 } { "DC" 2 } { "DE" 1 } { "EC" 2 } { "EF" 1 } } at ; + +: test2 ( fromto -- path considered ) + first2 [ n ] [ c ] [ 2drop 0 ] [ find-path ] [ considered natural-sort >string ] bi ; +>> + +! Check path from A to C -- all nodes but F must have been examined +[ "ADC" "ABCDE" ] [ "AC" test2 [ >string ] dip ] unit-test + +! No path from D to B -- all nodes reachable from D must have been examined +[ f "CDEF" ] [ "DB" test2 ] unit-test diff --git a/extra/astar/astar.factor b/extra/astar/astar.factor new file mode 100644 index 0000000000..1912b6af21 --- /dev/null +++ b/extra/astar/astar.factor @@ -0,0 +1,72 @@ +! Copyright (C) 2010 Samuel Tardieu. +! See http://factorcode.org/license.txt for BSD license. +USING: accessors assocs heaps kernel math sequences sets shuffle ; +IN: astar + +! This implements the A* algorithm. See http://en.wikipedia.org/wiki/A* + +> 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 -- ) + [ g>> at ] 2keep + [ [ goal>> ] [ heuristic>> call( n1 n2 -- c ) ] bi + ] 2keep + (add-to-open-set) ; + +: ?add-to-open-set ( node astar -- ) + 2dup in-closed-set>> key? [ 2drop ] [ add-to-open-set ] if ; + +: move-to-closed-set ( node astar -- ) + [ in-closed-set>> conjoin ] [ in-open-set>> delete-at ] 2bi ; + +: get-first ( astar -- node ) + [ open-set>> heap-pop drop dup ] [ move-to-closed-set ] bi ; + +: set-g ( origin g node astar -- ) + [ [ origin>> set-at ] [ g>> set-at ] bi-curry bi-curry bi* ] [ ?add-to-open-set ] 2bi ; + +: cost-through ( origin node astar -- cost ) + [ cost>> call( n1 n2 -- c ) ] [ nip g>> at ] 3bi + ; + +: ?set-g ( origin node astar -- ) + [ cost-through ] 3keep [ swap ] 2dip + 3dup g>> at [ 1/0. ] unless* > [ 4drop ] [ set-g ] if ; + +: build-path ( target astar -- path ) + [ over ] [ over [ [ origin>> at ] keep ] dip ] produce 2nip reverse ; + +: handle ( node astar -- ) + dupd [ neighbours>> call( node -- neighbours ) ] keep [ ?set-g ] curry with each ; + +: (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 >>g + H{ } clone >>origin + H{ } clone >>in-open-set + H{ } clone >>in-closed-set + >>open-set + [ 0 ] 2dip [ (add-to-open-set) ] [ g>> set-at ] 3bi ; + +PRIVATE> + +: find-path ( start target astar -- path/f ) + [ (init) ] [ (find-path) ] bi ; + +: ( neighbours cost heuristic -- astar ) + astar new swap >>heuristic swap >>cost swap >>neighbours ; + +: considered ( astar -- considered ) + in-closed-set>> keys ; diff --git a/extra/astar/authors.txt b/extra/astar/authors.txt new file mode 100644 index 0000000000..f3b0233f74 --- /dev/null +++ b/extra/astar/authors.txt @@ -0,0 +1 @@ +Samuel Tardieu diff --git a/extra/astar/summary.txt b/extra/astar/summary.txt new file mode 100644 index 0000000000..ff3167a133 --- /dev/null +++ b/extra/astar/summary.txt @@ -0,0 +1 @@ +A* path-finding algorithm