# Ref.: http://stackoverflow.com/questions/1489183/colorized-ruby-output

def black; "\033[30m#{self}\033[0m" end

def red; "\033[31m#{self}\033[0m" end

def green; "\033[32m#{self}\033[0m" end

def brown; "\033[33m#{self}\033[0m" end

def blue; "\033[34m#{self}\033[0m" end

def magenta; "\033[35m#{self}\033[0m" end

def cyan; "\033[36m#{self}\033[0m" end

def gray; "\033[37m#{self}\033[0m" end

def bg_black; "\033[40m#{self}\033[0m" end

def bg_red; "\033[41m#{self}\033[0m" end

def bg_green; "\033[42m#{self}\033[0m" end

def bg_brown; "\033[43m#{self}\033[0m" end

def bg_blue; "\033[44m#{self}\033[0m" end

def bg_magenta; "\033[45m#{self}\033[0m" end

def bg_cyan; "\033[46m#{self}\033[0m" end

def bg_gray; "\033[47m#{self}\033[0m" end

def bold; "\033[1m#{self}\033[22m" end

def reverse_color; "\033[7m#{self}\033[27m" end

def no_colors; self.gsub(/\033\[\d+m/, "") end

def start_color; self.bg_green.gray end

def goal_color; self.bg_magenta.gray end

def path_color; self.blue end

def obstacle_color; self.bg_black.gray end

def visited_color; self.cyan end

class Node

attr_accessor :id, :edges, :x, :y

def initialize(node_id)

@id = node_id

@edges = []

end

def neighbors

return Hash[@edges.collect {|edge| [edge.other(self), edge.cost] }] # node => edge.cost

end

#↓↓↓ for JPS ↓↓↓#

def neighbor_at(dx, dy) # d{x|y} = {-1, 0, 1}

if dx == 0 && dy == 0

return nil

else

edge = @edges.find {|edge| edge.other(self).x == (@x + dx) && edge.other(self).y == (@y + dy)}

return edge == nil ? nil : [edge.other(self), edge.cost]

end

end

def obstacle_at(dx, dy) # d{x|y} = {-1, 0, 1}

return neighbor_at(dx, dy) == nil

end

private :obstacle_at

CENTER = [ 0, 0]

NORTH = [ 0, -1] # ↑

NORTHEAST = [ 1, -1] # ↗

EAST = [ 1, 0] # →

SOUTHEAST = [ 1, 1] # ↘

SOUTH = [ 0, 1] # ↓

SOUTHWEST = [ -1, 1] # ↙

WEST = [ -1, 0] # ←

NORTHWEST = [ -1, -1] # ↖

def direction(dx, dy)

return case

when dx == -1 && dy == -1; NORTHWEST

when dx == -1 && dy == 0; WEST

when dx == -1 && dy == 1; SOUTHWEST

when dx == 0 && dy == -1; NORTH

when dx == 0 && dy == 0; CENTER

when dx == 0 && dy == 1; SOUTH

when dx == 1 && dy == -1; NORTHEAST

when dx == 1 && dy == 0; EAST

when dx == 1 && dy == 1; SOUTHEAST

end

end

private :direction

def natural_neighbors(dx, dy)

return neighbors() if dx == 0 && dy == 0 # self == Start Node, parent_node == nil

dirs = case direction(dx, dy)

when NORTHWEST; [NORTHWEST, NORTH, WEST] # NW

when WEST; [WEST] # W

when SOUTHWEST; [SOUTHWEST, SOUTH, WEST] # SW

when NORTH; [NORTH] # N

when SOUTH; [SOUTH] # S

when NORTHEAST; [NORTHEAST, NORTH, EAST] # NE

when EAST; [EAST] # E

when SOUTHEAST; [SOUTHEAST, SOUTH, EAST] # SE

end

nn = Hash.new

dirs.each do |dir|

neighbor = neighbor_at(*dir) # n = node => cost

nn.store(neighbor[0], neighbor[1]) unless neighbor == nil

end

return nn

end

private :natural_neighbors

def forced_neighbors(dx, dy)

return neighbors() if dx == 0 && dy == 0 # self == Start Node, parent_node == nil

forced = [] # Array of [node, node_cost] information

case direction(dx, dy)

when NORTHWEST

forced << neighbor_at(*NORTHEAST) if obstacle_at(*WEST) # if West is obstacle then mark NorthEast as forced

forced << neighbor_at(*SOUTHWEST) if obstacle_at(*SOUTH) # if South is obstacle then mark SouthWest as forced

when WEST

forced << neighbor_at(*NORTHWEST) if obstacle_at(*NORTH) # if North is obstacle then mark NorthWest as forced

forced << neighbor_at(*SOUTHWEST) if obstacle_at(*SOUTH) # if South is obstacle then mark SouthWest as forced

when SOUTHWEST

forced << neighbor_at(*NORTHWEST) if obstacle_at(*NORTH) # if North is obstacle then mark NorthWest as forced

forced << neighbor_at(*SOUTHEAST) if obstacle_at(*EAST) # if East is obstacle then mark SouthEast as forced

when NORTH

forced << neighbor_at(*NORTHEAST) if obstacle_at(*EAST) # if East is obstacle then mark NorthEast as forced

forced << neighbor_at(*NORTHWEST) if obstacle_at(*WEST) # if West is obstacle then mark NorthWest as forced

when SOUTH

forced << neighbor_at(*SOUTHEAST) if obstacle_at(*EAST) # if East is obstacle then mark SouthEast as forced

forced << neighbor_at(*SOUTHWEST) if obstacle_at(*WEST) # if West is obstacle then mark SouthWest as forced

when NORTHEAST

forced << neighbor_at(*SOUTHEAST) if obstacle_at(*SOUTH) # if South is obstacle then mark SouthEast as forced

forced << neighbor_at(*NORTHWEST) if obstacle_at(*WEST) # if West is obstacle then mark NorthWest as forced

when EAST

forced << neighbor_at(*NORTHEAST) if obstacle_at(*NORTH) # if North is obstacle then mark NorthEast as forced

forced << neighbor_at(*SOUTHEAST) if obstacle_at(*SOUTH) # if South is obstacle then mark SouthEast as forced

when SOUTHEAST

forced << neighbor_at(*NORTHEAST) if obstacle_at(*NORTH) # if North is obstacle then mark NorthEast as forced

forced << neighbor_at(*SOUTHWEST) if obstacle_at(*WEST) # if West is obstacle then mark SouthWest as forced

end

fn = Hash.new

forced.each do |node_and_cost| # node => cost

fn.store(node_and_cost[0], node_and_cost[1]) unless node_and_cost == nil

end

return fn

end

def pruned_neighbors(parent_node)

dx = parent_node == nil ? 0 : clamp(@x - parent_node.x, -1, 1)

dy = parent_node == nil ? 0 : clamp(@y - parent_node.y, -1, 1)

nn = natural_neighbors(dx, dy)

fn = forced_neighbors(dx, dy)

return nn.merge(fn)

end

#↑↑↑ for JPS ↑↑↑#

end

################################################################################

class Edge

attr_accessor :cost, :node_id, :nodes

def initialize(cost, node_id0, node_id1)

@cost = cost

@node_id = [node_id0, node_id1]

@nodes = [nil, nil]

end

def other(node)

return node == @nodes[0] ? @nodes[1] : @nodes[0]

end

end

################################################################################

class Route

attr_accessor :found, :start_id, :goal_id

attr_reader :map_width, :map_height

def initialize

@parents = Hash.new # child -> parent

@real_costs = Hash.new # child -> real cost

@heuristic_costs = Hash.new # child -> heuristic cost

reset()

end

def reset

@parents.clear

@real_costs.clear

@heuristic_costs.clear

@found = false

@start_id = ""

@goal_id = ""

end

def record(child, parent, real_cost, heuristic_cost)

@parents[child] = parent

@real_costs[child] = real_cost

@heuristic_costs[child] = heuristic_cost

end

def parent(child)

return @parents.has_key?(child) ? @parents[child] : nil

end

def cost(node, eval_heuristic: true)

if @real_costs.has_key?(node)

return eval_heuristic ? @real_costs[node] + @heuristic_costs[node] : @real_costs[node]

else

return Float::MAX

end

end

def estimated_cost(node); return cost(node, eval_heuristic: true); end

def real_cost(node); return cost(node, eval_heuristic: false); end

def get_path()

return [] if not @found

path = []

parent = @parents.keys.find {|child| child.id == @goal_id}

while parent != nil

path.unshift(parent)

parent = @parents[parent]

end

return path

end

end

################################################################################

def initialize

@road_map = [] # Array of Edge

@nodes = Hash.new

@route = Route.new

@use_heuristic = true

@search_iter = 0

@obstacles = [] # Array of Node ID

@visited = [] # Node ID

end

def read_roadmap(roadmap_input)

csv_lines = roadmap_input.readlines

nodes_count, edges_count, obstacles_count = csv_lines.shift.split(",").collect{|val| val.strip.to_i}

# Nodes

nodes_count.times do |i|

id, x, y = csv_lines.shift.split(",")

@nodes[id] = Node.new(id)

@nodes[id].x = x.to_i

@nodes[id].y = y.to_i

end

@render_map_width = @nodes.values.map{|n| n.x }.max + 1

@render_map_height = @nodes.values.map{|n| n.y }.max + 1

@render_cell_width = @nodes.values.map{|n| n.id.length }.max + 1

# Edges

edges_count.times do |i|

cost, node0_id, node1_id = csv_lines.shift.split(",")

@road_map << Edge.new(cost.to_f, node0_id.strip, node1_id.strip)

end

# Obstacles

@obstacles.clear

obstacles_count.times do |i|

@obstacles << csv_lines.shift.strip

end

@obstacles.each do |node_id|

@road_map = @road_map.reject {|edge| edge.node_id.include?(node_id)}

end

end

def setup_route

@road_map.each do |edge|

edge.node_id.each_with_index do |id, index|

@nodes[id] = Node.new(id) unless @nodes.has_key? id

edge.nodes[index] = @nodes[id]

end

end

@road_map.each do |edge|

@nodes[edge.node_id[0]].edges << edge

@nodes[edge.node_id[1]].edges << edge

end

end

@@heuristic_Zero = Proc.new { |node, goal| 0.0 } # Always 0 (Dijkstra's algorithm)

@@heuristic_MaxDiff = Proc.new { |node, goal| [(node.x - goal.x).abs, (node.y - goal.y).abs].max } # h = max(|dx|, |dy|)

@@heuristic_Manhattan = Proc.new { |node, goal| (node.x - goal.x).to_f.abs + (node.y - goal.y).to_f.abs } # Manhattan Distance (for non-diagonal map only)

@@heuristic_Pythagorean = Proc.new { |node, goal| Math.sqrt((node.x - goal.x)**2 + (node.y - goal.y)**2) } # Pythagorean theorem

def reset(start_id, goal_id, use_heuristic: true, use_jps: true)

@route.reset

@route.start_id = start_id

@route.goal_id = goal_id

@route.record(@nodes[start_id], nil, 0.0, 0.0)

@use_heuristic = use_heuristic

@heuristic = use_heuristic ? @@heuristic_MaxDiff : @@heuristic_Zero

@use_jps = use_jps

@visited.clear

@search_iter = 0

end

#↓↓↓ for JPS ↓↓↓#

# See "Algorithm 2 Function jump"

def jump(x, dx, dy)

node_and_cost = x.neighbor_at(dx, dy)

return nil if node_and_cost == nil

n = node_and_cost[0]

return n if n == @nodes[@route.goal_id]

return n if n.forced_neighbors(dx, dy).length > 0

if dx != 0 && dy != 0

return n if jump(n, dx, 0) != nil

return n if jump(n, 0, dy) != nil

end

return jump(n, dx, dy)

end

#

# Jump Point Search (JPS) Implementation

#

def search_jps

open_list = [@nodes[@route.start_id]]

close_list = []

goal = @nodes[@route.goal_id]

until open_list.empty?

n = open_list.min_by { |node| @route.estimated_cost(node) }

if n == goal

@route.found = true

break

end

close_list.push( open_list.delete(n) )

adjacents_of_n = n.pruned_neighbors(@route.parent(n))

adjacents_of_n.keys.each do |m|

j = jump(n, clamp(m.x - n.x, -1, 1), clamp(m.y - n.y, -1, 1))

next if j == nil or close_list.include?(j)

h = @heuristic.call(j, goal)

new_real_cost_j = @route.real_cost(n) + Math.sqrt((n.x-j.x)**2 + (n.y-j.y)**2) # g

new_estimated_cost_j = new_real_cost_j + h # f = g + h

if open_list.include?(j)

# If estimated costs are equal then use real costs for more precise comparison (or we may get less optimal path).

next if new_estimated_cost_j > @route.estimated_cost(j)

next if new_estimated_cost_j == @route.estimated_cost(j) && new_real_cost_j >= @route.real_cost(j)

@route.record(j, n, new_real_cost_j, h)

else

open_list.push(j)

@route.record(j, n, new_real_cost_j, h)

end

@visited << j.id unless @visited.include? j.id # stats

end

@search_iter += 1 # stats

end

end

#↑↑↑ for JPS ↑↑↑#

#

# A* Implementation

#

def search_astar

open_list = [@nodes[@route.start_id]]

close_list = []

goal = @nodes[@route.goal_id]

until open_list.empty?

n = open_list.min_by { |node| @route.estimated_cost(node) }

if n == goal

@route.found = true

break

end

close_list.push( open_list.delete(n) )

adjacents_of_n = n.neighbors # Hash [neighbor_node => edge.cost]

adjacents_of_n.each do |m, edge_cost|

next if close_list.include?(m)

h = @heuristic.call(m, goal)

new_real_cost_m = @route.real_cost(n) + edge_cost # g

new_estimated_cost_m = new_real_cost_m + h # f = g + h

if open_list.include?(m)

# If estimated costs are equal then use real costs for more precise comparison (or we may get less optimal path).

next if new_estimated_cost_m > @route.estimated_cost(m)

next if new_estimated_cost_m == @route.estimated_cost(m) && new_real_cost_m >= @route.real_cost(m)

@route.record(m, n, new_real_cost_m, h)

else

open_list.push(m)

@route.record(m, n, new_real_cost_m, h)

end

end

# stats

adjacents_of_n.each_key {|n| @visited << n.id unless @visited.include? n.id}

@search_iter += 1

end

end

def search

@use_jps ? search_jps : search_astar

end

def found; return @route.found; end

def get_path; return @route.get_path; end

def goal_id; return @route.goal_id; end

def direction_symbol(next_node, current_node)

case

when next_node.x < current_node.x

case

when next_node.y < current_node.y; "↖" # U+2196

when next_node.y == current_node.y; "←"

when next_node.y > current_node.y; "↙" # U+2199

end

when next_node.x == current_node.x

case

when next_node.y < current_node.y; "↑"

when next_node.y == current_node.y; " "

when next_node.y > current_node.y; "↓"

end

when next_node.x > current_node.x

case

when next_node.y < current_node.y; "↗" # U+2197

when next_node.y == current_node.y; "→"

when next_node.y > current_node.y; "↘" # U+2198

end

end

end

private :direction_symbol

def render_cell(path, n)

if n == nil

print "_" * (@render_cell_width - 1)

else

colorizer = case

when n.id == @route.start_id; :start_color

when n.id == @route.goal_id; :goal_color

when @obstacles.include?(n.id); :obstacle_color

when path.find{|node| node.id == n.id} != nil; :path_color

when @visited.include?(n.id); :visited_color

else :no_colors

end

path_index = path.find_index(n)

symbol = n.id + case

when path_index == nil; ""

when n.id == @route.start_id || colorizer == :path_color; direction_symbol(path[path_index+1], n)

when n.id == @route.goal_id; "•" # U+2022

else ""

end

print %Q{#{symbol}#{" " * (@render_cell_width - symbol.length)}}.send(colorizer)

end

end

private :render_cell

def render

puts "#iteration = #{@search_iter} [@use_heuristic == #{@use_heuristic}, @use_jps == #{@use_jps}]"

path = self.get_path

if path != nil

@render_map_height.times do |h|

@render_map_width.times do |w|

n = @nodes.values.find{|n| n.x == w && n.y == h}

render_cell(path, n)

end # End : @render_map_width.times do |w|

puts

end # End : @render_map_height.times do |h|

end

end