# 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_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)
end

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_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)
# - 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

``````

``````
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 :: 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]
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))
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)

{-  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 :: Pos -> Pos -> Pos

-- 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
-- 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.