A* algorithm implementation
Samuel Tardieu
parent
afaaf30679
commit
eff65915b0
|
|
@ -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 <astar> considered } related-words
|
||||||
|
|
||||||
|
HELP: <astar>
|
||||||
|
{ $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 <astar> } "."
|
||||||
|
} ;
|
||||||
|
|
||||||
|
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."
|
||||||
|
} ;
|
||||||
|
|
@ -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 ] <astar> [ 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 ] <astar> [ 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
|
||||||
|
|
@ -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*
|
||||||
|
|
||||||
|
<PRIVATE
|
||||||
|
|
||||||
|
TUPLE: astar neighbours heuristic cost
|
||||||
|
goal g origin in-open-set in-closed-set open-set ;
|
||||||
|
|
||||||
|
: (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 -- )
|
||||||
|
[ 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
|
||||||
|
<min-heap> >>open-set
|
||||||
|
[ 0 ] 2dip [ (add-to-open-set) ] [ g>> set-at ] 3bi ;
|
||||||
|
|
||||||
|
PRIVATE>
|
||||||
|
|
||||||
|
: find-path ( start target astar -- path/f )
|
||||||
|
[ (init) ] [ (find-path) ] bi ;
|
||||||
|
|
||||||
|
: <astar> ( neighbours cost heuristic -- astar )
|
||||||
|
astar new swap >>heuristic swap >>cost swap >>neighbours ;
|
||||||
|
|
||||||
|
: considered ( astar -- considered )
|
||||||
|
in-closed-set>> keys ;
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
Samuel Tardieu
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
A* path-finding algorithm
|
||||||
Loading…
Reference in New Issue