#
# 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!"
endimport 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) -- (x,y)
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)
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,0,1,1,0] ]
my_map = Map (0,0) (4,4) my_rows
-- Main
main = print (astar my_map)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.