From eff65915b0cf86f4b5ef39ccbd7b9a0402415e11 Mon Sep 17 00:00:00 2001 From: Samuel Tardieu Date: Sat, 6 Mar 2010 08:48:39 +0100 Subject: [PATCH 1/4] A* algorithm implementation --- extra/astar/astar-docs.factor | 42 +++++++++++++ extra/astar/astar-tests.factor | 109 +++++++++++++++++++++++++++++++++ extra/astar/astar.factor | 72 ++++++++++++++++++++++ extra/astar/authors.txt | 1 + extra/astar/summary.txt | 1 + 5 files changed, 225 insertions(+) create mode 100644 extra/astar/astar-docs.factor create mode 100644 extra/astar/astar-tests.factor create mode 100644 extra/astar/astar.factor create mode 100644 extra/astar/authors.txt create mode 100644 extra/astar/summary.txt 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..2567ad046d --- /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 ; + +: cost ( from to -- cost ) + 2dup [ first ] bi@ = [ [ second ] bi@ > 1 5 ? ] [ 2drop 2 ] if ; + +: heuristic ( pos1 pos2 -- distance ) + v- [ sq ] map sum sqrt ; + +: 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..6a5c431ae4 --- /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 arrays assocs heaps kernel math math.order 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 From 542096b5286e7fc40e038086c3933ce49208a3a6 Mon Sep 17 00:00:00 2001 From: Samuel Tardieu Date: Sat, 6 Mar 2010 14:14:54 +0100 Subject: [PATCH 2/4] Use distance from math.vectors --- extra/astar/astar-tests.factor | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/extra/astar/astar-tests.factor b/extra/astar/astar-tests.factor index 2567ad046d..6dd27cf372 100644 --- a/extra/astar/astar-tests.factor +++ b/extra/astar/astar-tests.factor @@ -37,11 +37,8 @@ IN: astar.tests : cost ( from to -- cost ) 2dup [ first ] bi@ = [ [ second ] bi@ > 1 5 ? ] [ 2drop 2 ] if ; -: heuristic ( pos1 pos2 -- distance ) - v- [ sq ] map sum sqrt ; - : test1 ( to -- path considered ) - { 1 1 } swap [ neighbours ] [ cost ] [ heuristic ] [ find-path ] [ considered ] bi ; + { 1 1 } swap [ neighbours ] [ cost ] [ distance ] [ find-path ] [ considered ] bi ; >> ! Existing path from s to f From 0e35c883aeb04ceed9c8b1bbde8183b08d6888bd Mon Sep 17 00:00:00 2001 From: Samuel Tardieu Date: Sat, 6 Mar 2010 14:31:46 +0100 Subject: [PATCH 3/4] Remove useless USING: --- extra/astar/astar.factor | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extra/astar/astar.factor b/extra/astar/astar.factor index 6a5c431ae4..1912b6af21 100644 --- a/extra/astar/astar.factor +++ b/extra/astar/astar.factor @@ -1,6 +1,6 @@ ! Copyright (C) 2010 Samuel Tardieu. ! See http://factorcode.org/license.txt for BSD license. -USING: accessors arrays assocs heaps kernel math math.order sequences sets shuffle ; +USING: accessors assocs heaps kernel math sequences sets shuffle ; IN: astar ! This implements the A* algorithm. See http://en.wikipedia.org/wiki/A* From adcf50514c6334aca97c1830553c427411507256 Mon Sep 17 00:00:00 2001 From: Samuel Tardieu Date: Sat, 6 Mar 2010 14:37:35 +0100 Subject: [PATCH 4/4] Use a better heuristic --- extra/astar/astar-tests.factor | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/extra/astar/astar-tests.factor b/extra/astar/astar-tests.factor index 6dd27cf372..11b2dfcaa2 100644 --- a/extra/astar/astar-tests.factor +++ b/extra/astar/astar-tests.factor @@ -34,11 +34,14 @@ IN: astar.tests 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 ] [ distance ] [ find-path ] [ considered ] bi ; + { 1 1 } swap [ neighbours ] [ cost ] [ heuristic ] [ find-path ] [ considered ] bi ; >> ! Existing path from s to f