# Implement A* Shortest Path Algorithm

## Ruby Implementation

```
#
# A* Implementation
#
# References:
# - http://www.policyalmanac.org/games/aStarTutorial.htm
# - http://theory.stanford.edu/~amitp/GameProgramming/
#
class Point
attr_reader :r, :c
attr_writer :r, :c
def initialize(r, c)
@r = r; @c = c
end
def ==(point)
@r == point.r && @c == point.c
end
def distance(point)
Math.sqrt((point.r-r)**2+(point.c-c)**2)
end
def to_s
"(#{r},#{c})"
end
end
class Map
# An array of rows where 1=taken, 0=free [[row0],[row1], ...] where row0 = [1,0,0,1,...]
def initialize(rows)
@rows = rows
end
# Location within map with value == 0
def free?(point)
point.r >= 0 && point.c >= 0 && @rows[point.r] && @rows[point.r][point.c] == 0
end
def free_neighbours(point)
all_neighbours(point).find_all { |p| free?(p) }
end
def all_neighbours(point)
adjectant_neighbours(point) + diagonal_neighbours(point)
end
def adjectant_neighbours(point)
r = point.r; c = point.c
[[r-1,c],[r,c-1],[r,c+1],[r+1,c]].collect { |p| Point.new(*p) }
end
def diagonal_neighbours(point)
r = point.r; c = point.c
[[r-1,c-1],[r-1,c+1],[r+1,c-1],[r+1,c+1]].collect { |p| Point.new(*p) }
end
end
class AStar
class Square
attr_reader :point, :parent, :g, :goal
attr_writer :point, :parent, :g, :goal
def initialize(point, parent, goal, delta_g = 1)
@point = point; @parent = parent; @goal = goal;
@g = parent ? parent.g + delta_g : 0
end
# Sum of current path length (g) and heuristic distance to goal (h)
def f
g+h
end
# Heuristic distance to goal point, h(x)
# Improve this one with a more efficient one
# http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
def h
point.distance(goal)
end
end
def initialize(map, start, goal)
@map = map; @start = start; @goal = goal
clear
end
def clear
@open_list = [] # Lists of squares
@closed_list = []
@found = false
end
def solve
clear
@open_list << Square.new(@start.dup, nil, @goal) # Add start node
until (@found || @open_list.empty?)
# Identify current square in open_list with least f. Move current to closed_list
current = @open_list.min { |a,b| a.f <=> b.f }
@open_list.delete(current); @closed_list << current
# Check if goal has been reached
if current.point == @goal then
@found = true
break
end
# For all current's walkable neighbours that haven't been closed (i.e. not in closed_list)
# - Add to open_list if not in open list already
# - Update g and parent if already in open list and candidate g is lower
@map.free_neighbours(current.point).each { |p|
next if @closed_list.any? { |s| s.point == p }
candidate = Square.new(p, current, @goal)
existing = @open_list.detect { |s| s.point == p }
if not existing then
@open_list << candidate
elsif candidate.g < existing.g
existing.g = candidate.g
existing.parent = candidate.parent
end
}
end
@found
end
def shortest_path
return nil unless @found
path = []
path << @closed_list.detect { |s| s.point == @goal }
until path.last.point == @start
path << path.last.parent
end
path.reverse!.collect { |s| s.point }
end
end
# Main program ...
rows = [ [0,1,0,0,0],
[0,1,0,1,0],
[0,0,0,1,0],
[1,1,1,1,0],
[0,0,0,0,0] ]
map = Map.new(rows)
start = Point.new(0,0)
goal = Point.new(4,4)
astar = AStar.new(map, start, goal)
if astar.solve
puts "Optimal path is ..."
astar.shortest_path.each {|p| puts p.to_s }
else
puts "Path cannot be found!"
end
```

## Haskell Implementation

```
import Data.Maybe
import Data.List (minimumBy, sortBy, nub, (\\), find, intersect)
import Data.Ord (comparing)
import Debug.Trace (trace)
{- Second AStar implementation in Haskell
compile: ghc -o astar2 astar2.hs
run: ./astar2 -}
type Location = (Int, Int) -- (col,row)
type Locations = [ Location ]
type Distance = Float
data Map = Map { start :: Location,
goal :: Location,
rows :: [ [ Int ] ] } deriving ( Show )
data Node = Node { location :: Location,
g :: Distance,
h :: Distance,
parent :: Maybe Location } deriving ( Show )
type Nodes = [ Node ]
instance Eq Node where
a == b = (location a) `eq` (location b) where
eq (ax,ay) (bx,by) = (ax == bx) && (ay == by)
-- Heuristic distance from start to goal via node
f :: Node -> Distance
f n = (g n) + (h n)
-- Heuristic distance between points
distance :: Location -> Location -> Distance
distance from to = sqrt ( (fromIntegral((fst to)-(fst from)))^2 + (fromIntegral((snd to)-(snd from)))^2 )
-- Add point to another
add :: Location -> Location -> Location
add p1 p2 = (fst p1 + fst p2, snd p1 + snd p2)
-- Subtract lists
delete :: (Eq a) => [a] -> [a] -> [a]
delete as bs = [ a | a <- as, notElem a bs ]
-- Define all possible neighbours around a point
neighbours :: Location -> [Location]
neighbours l = map (add l) (adj ++ diag) where
adj = [(-1,0),(1,0),(0,-1),(0,1)]
diag = [(-1,-1),(-1,1),(1,-1),(1,1)]
-- Check if point is walkable
walkable :: Map -> Location -> Bool
walkable m p = inside && (rs !! r !! c) == 0 where
c = snd p
r = fst p
rs = rows m
inside = r `btw` (0,length rs) && c `btw` (0,length (rs !! r))
btw n (a,b) = (a <= n) && (n < b)
-- Find walkable neighbours around point
walkable_neighbours :: Map -> Location -> Locations
walkable_neighbours m l = [ x | x <- neighbours l, walkable m x]
-- A* Calculation
type Open = Nodes -- list for working location set. Parent may be updated for these points
type Closed = Nodes -- list for finished location set
type Path = Locations -- resulting path from start to goal
type Found = Bool
-- Create a set of children nodes on a set of locations of a common parent
children :: Map -> Distance -> Node -> Locations -> Nodes
children m dg par [] = []
children m dg par (l:ls) = n:(children m dg par ls) where
n = Node l (dg+(g par)) (distance l (goal m)) (Just (location par))
-- lookup node from ns at location l
node :: Location -> Nodes -> (Maybe Node)
node l ns = find ((== l) . location) ns
-- Replace nodes in as with corresponding (with same location) node in bs if g is better
update_nodes :: Nodes -> Nodes -> Nodes
update_nodes [] bs = []
update_nodes as [] = as
update_nodes (a:as) bs = (lower_g a bs):(update_nodes as bs) where
lower_g n ns = foldr lower n ns
lower a b | ((location a == location b) && (g b < g a)) = b
| otherwise = a
astar' :: Map -> Open -> Closed -> Found -> Path
astar' m _ cs True = shortest_path m cs [] -- Path is found. Return shortest path
astar' m [] cs False = trace ("No path found, cs: " ++ show cs) [] -- No path found
astar' m os cs False = astar' m os''' cs' found where -- Main iteration
cur = minimumBy (comparing f) os
os' = os `delete` [cur]
cs' = cur:cs
children' = children m 1 cur ls where
ls = (walkable_neighbours m (location cur)) `delete` cls
cls = map location cs'
os'' = update_nodes os' (children' `intersect` os')
os''' = os'' ++ (children' \\ os'')
found = location cur == goal m
astar :: Map -> Path
astar m = astar' m [start_node] [] False where
start_node = Node (start m) 0 (distance (start m) (goal m)) Nothing
-- Extract shortest path from closed list
shortest_path :: Map -> Nodes -> Path -> Path
shortest_path m cs [] = shortest_path m cs [goal m]
shortest_path m cs (p:ps) | (p == start m) = p:ps
| otherwise = shortest_path m cs (add_parent (node p cs))
where add_parent Nothing = p:ps
add_parent (Just (Node _ _ _ Nothing ) ) = p:ps
add_parent (Just (Node _ _ _ (Just par) ) ) = par:p:ps
-- Define world
my_rows = [ [0,0,1,0,0],
[0,1,1,0,0],
[0,1,0,1,0],
[0,1,0,1,0],
[0,1,0,1,0],
[0,1,0,1,0],
[0,0,1,1,0] ]
my_map = Map (0,0) (6,4) my_rows
-- Main
main = print (astar my_map)
```

After some time I looked at this implementation again and the solution in Haskell was not obvious. So I made a third attempt to simplify the implementation.

In this solution I use a `Map`

(with positions as keys and position metadata as values) to represent the search graph. The position metadata is stored in a `Node`

type.

```
data Node = Node { parent :: Maybe Pos, closed :: Bool }
```

That is both open and closed positions in the tree is represented in a single map data structure.

I think the implementation got a little more clear.

```
import Data.Maybe
import Data.List as L
import Data.Map.Strict as M
import Data.Ord (comparing)
import Control.Monad (join)
{- Third AStar implementation in Haskell
compile: ghc AStar3.hs
run: ./AStar3 -}
type Pos = (Int, Int) -- (col,row)
type Start = Pos
type Goal = Pos
type Dist = Float
type PMap = [ Pos ] -- list of walkable positions
type Path = [ Pos ]
data Node = Node { parent :: Maybe Pos, closed :: Bool } deriving (Show)
type Graph = M.Map Pos Node -- search graph represented as map
-- Dist to start
g :: Graph -> Node -> Dist
g _ (Node Nothing _) = 0
g w (Node (Just p) _) = 1 + g w (fromJust $ M.lookup p w)
-- Minimal Dist to goal
h :: Pos -> Goal -> Dist
h a b = (fromIntegral (fst b - fst a))^2 + (fromIntegral (snd b - snd a))^2
-- Minimal Dist from start to goal for node at posision
f :: Graph -> Goal -> Pos -> Dist
f w goal p = g w (fromJust $ M.lookup p w) + h p goal
-- Add point to another
add :: Pos -> Pos -> Pos
add (a,b) (c,d) = (a+c,b+d)
-- Find available neighbours around point
neighbours :: PMap -> Pos -> [Pos]
neighbours m l = L.filter (`elem` m) $ L.map (add l) ds where
ds = [ (r,c) | r <- [-1,0,1], c <- [-1,0,1], (r,c) /= (0,0)]
initGraph :: Start -> Graph
initGraph start = M.fromList [(start, Node Nothing False)]
-- A* calculation
astar :: PMap -> Start -> Goal -> Maybe Path
astar m start goal = case astar' m start goal [] of
Nothing -> Nothing
Just [] -> Nothing
Just (w:_) -> Just $ getPath w goal []
-- A* iteration loop where each intermediate result is kept
astar' :: PMap -> Start -> Goal -> [Graph] -> Maybe [Graph]
astar' m start goal [] = astar' m start goal [initGraph start]
astar' m start goal ws | M.member goal w = Just ws
| L.null os = Nothing
| otherwise = astar' m start goal (w''':ws)
where
w = head ws
-- find open location with lowest f (current)
os = M.keys $ M.filter (not . closed) w
p_cur = minimumBy (comparing (f w goal)) os
n_cur = fromJust $ M.lookup p_cur w
-- switch node state of current to closed
w' = M.insert p_cur (n_cur { closed = True } ) w
-- make new child nodes around current
ns = neighbours m p_cur
ps = L.filter (\p -> not $ M.member p w) ns
nw = M.fromList $ zip ps (repeat (Node (Just p_cur) False))
-- update existing open neighbour nodes with cur as parent if g is lower
w'' = M.mapWithKey updateNode w'
updateNode :: Pos -> Node -> Node
updateNode p n | closed n || L.notElem p ns = n
| otherwise = if (1 + g w n_cur) < g w n
then n { parent = Just p_cur }
else n
w''' = M.union nw w''
printGraph :: (Int,Int) -> Graph -> String
printGraph (rows,cols) w = L.foldl (\s (r,c) -> s ++ printState (r,c)
++ if c == (cols-1) then "\n" else "") "" ps where
ps = sort $ [(r,c) | r <- [0..(rows-1)], c <- [0..(cols-1)]]
printState :: Pos -> String
printState p = case M.lookup p w of Nothing -> "."
Just (Node _ c) -> if c then "x" else "o"
-- Get path from working graph where goal has been found
getPath :: Graph -> Goal -> Path -> Path
getPath w goal [] = getPath w goal [goal]
getPath w goal (r:rs) = case par of Just p -> getPath w goal (p:r:rs)
Nothing -> r:rs
where par = parent $ fromJust $ M.lookup r w
-- Create map from list of rows where 0 is empty and 1 is blocked
-- Resulting map contains list of all empty posistions
createMap :: [[Int]] -> PMap
createMap rs = join $ fmap (uncurry doRow) $ zip [0..] rs where
doRow :: Int -> [Int] -> PMap
doRow r cs = L.foldl (\res (c,v) -> if v == 0 then (r,c):res else res) [] $ L.zip [0..] cs
-- Define world
my_rows = [ [0,0,1,0,0],
[0,1,1,0,0],
[0,1,0,1,0],
[0,1,0,1,0],
[0,1,0,1,0],
[0,1,0,1,0],
[0,0,1,1,0] ]
my_map = createMap my_rows
-- Main
main :: IO ()
main = do
let ws = fromJust $ astar' my_map (0,0) (6,4) []
putStr $ join $ L.map ((++ "\n") . printGraph (7,5)) $ L.reverse ws
print $ show $ fromJust $ astar my_map (0,0) (6,4)
```

## Multiple Path Extension

The idea is to solve the problem of multiple paths where each path cannot cross another. The total shortest path should be optimized.

Solution TBD.

## Use AStar for a Tetris bot implementation

There is a AI challenge to implement an AI bot to play tetris against other players.

I think you could use A* search to find the shortest "path" to transform a Tetris piece from one place to another in a Tetris game.

A step in this case would be one of the following operations.

- Rotate clockwise
- Rotate counter clockwise
- Move left
- Move right
- Drop to bottom

Note that the piece is also moving downwards which makes the problem a bit more complicated.

A second necessary task for a bot is of course to identify an advantages target location for each new piece.

## References

- Haskell Programming

Revision Exercise

All Paths Shortest Paths - A Simple Implementation Technique for Priority Search Queues
- Getting from A to B - More in depth pathfinding by Simon Peyton Jones