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.

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