Simple pathfinder visualizer

pathfind.cr

require "crsfml"
require "option_parser"

TILE_SIZE = 32_u32

alias Point = Tuple(UInt32, UInt32)

ARROW_SCALE = {32, 32}

CONF = {
	:show_arrows => true,
	:verbose => false,
	:show_weights => false
}

def log(s)
	puts s if CONF[:verbose]
end

class Element
	include Comparable(Element)

	property value, priority

	def initialize(@value : Point, @priority : Int32)
	end

	def <=>(other)
		@priority <=> other.priority
	end
end

class PriorityQueue
	def initialize
		@elements = [] of Element
	end

	def <<(element)
		@elements << element
	end

	def pop
		last_element_index = @elements.size - 1
		@elements.sort!
		@elements.delete_at(last_element_index)
	end

	def empty?
		@elements.empty?
	end
end


class Grid
	getter rows, cols
	property path

	def initialize(@rows : UInt32, @cols : UInt32)
		@tiles = [] of Int32
		@path = [] of Point
		@came_from = {} of Point => Point?
	end

	def clear
		@tiles.clear
		self
	end

	def fill(density = 0.15)
		(@rows * @cols).times do
			r = rand
			@tiles << (r < density ? 0 : (r * 10).to_i)
		end
	end

	def tile(x, y)
		@tiles[y * @cols + x]
	end
	
	def set(x, y, v)
		@tiles[y * @cols + x] = v
	end

	private def rotate_arrow(sprite, x, y)
		return false unless @came_from.has_key?({x, y})
		cf = @came_from[{x, y}] 
		return false if cf == nil 
		cf = cf as Point
		if cf[0] < x
			sprite.rotation = 270
		elsif cf[1] > y
			sprite.rotation = 180
		elsif cf[0] > x
			sprite.rotation = 90
		else
			sprite.rotation = 0
		end
		true
	end

	def draw(window, states)
		rect = SF::RectangleShape.new(SF.vector2f(TILE_SIZE, TILE_SIZE))
		sprite = SF::Sprite.new
		if CONF[:show_arrows]
			sprite.origin = {TILE_SIZE.to_f32/2, TILE_SIZE.to_f32/2}
			sprite.texture = $arrow
			sprite.texture_rect = SF.int_rect(0, 0, ARROW_SCALE[0], ARROW_SCALE[1])
			sprite.scale = SF.vector2f(TILE_SIZE.to_f32 / ARROW_SCALE[0], TILE_SIZE.to_f32 / ARROW_SCALE[1])
		end
		@rows.times do |y|
			@cols.times do |x|
				pos = SF.vector2f(x * TILE_SIZE, y * TILE_SIZE)
				tile = tile(x, y)
				if CONF[:show_arrows]
					sprite.position = SF.vector2f(
						pos.x+TILE_SIZE.to_f32/2,
						pos.y+TILE_SIZE.to_f32/2)
					show_arrow = rotate_arrow(sprite, x, y)
				end
				rect.position = pos
				rect.fill_color = 
					if !pathable(tile)
						SF::Color::Red
					else
						if @path.includes?({x, y})
							if @path[0] == {x, y}
								SF::Color::Green
							elsif @path[-1] == {x, y}
								SF::Color::Magenta
							else
								SF::Color::Blue
							end
						else
							#SF.color(tile, tile, tile)
							SF.color(210, 210, 210)
						end
					end
				if CONF[:show_weights]
					c = rect.fill_color
					rect.fill_color = SF.color(c.r, c.g, c.b, 100 + 10* tile)
				end
				window.draw(rect, states)
				window.draw(sprite, states) if CONF[:show_arrows] && pathable(tile) && show_arrow
			end
		end
	end

	def neighbors(x, y)
		n = [] of Point
		n << {x - 1, y} unless x == 0 || !pathable(tile(x - 1, y))
		n << {x, y - 1} unless y == 0 || !pathable(tile(x, y - 1))
		n << {x + 1, y} unless x == @cols - 1 || !pathable(tile(x + 1, y))
		n << {x, y + 1} unless y == @rows - 1 || !pathable(tile(x, y + 1))
		log "neighbours of #{x}, #{y} = #{n}"
		n
	end

	def pathfind(start, goal, algorithm = "early_exit_breadth_first")
		case algorithm
		when "breadth_first"
			pathfind_breadth_first(start, goal)
		when "early_exit_breadth_first"
			pathfind_early_exit_breadth_first(start, goal)
		when "dijkstra"
			pathfind_dijkstra(start, goal)
		else
			raise "Unknown algorithm: #{algorithm}"
		end
	end

	private def cost(a, b)
		tile(*b) - tile(*a)
	end
	
	private def pathable(tile)
		tile != 0
	end

	private def pathfind_early_exit_breadth_first(start, goal)
		frontier = [start]
		@came_from = {} of Point => Point?
		@came_from[start] = nil

		while !frontier.empty?
			current = frontier.shift
			break if current == goal
			log "current = #{current}"
			neighbors(*current).each do |nxt|
				next if @came_from.has_key? nxt
				log "next = #{nxt}"
				frontier << nxt
				@came_from[nxt] = current
			end
		end

		log "came_from = #{@came_from}"
		@came_from
	end

	private def pathfind_breadth_first(start, goal)
		frontier = [start]
		@came_from = {} of Point => Point?
		@came_from[start] = nil

		while !frontier.empty?
			current = frontier.shift
			log "current = #{current}"
			neighbors(*current).each do |nxt|
				next if @came_from.has_key? nxt
				log "next = #{nxt}"
				frontier << nxt
				@came_from[nxt] = current
			end
		end

		log "came_from = #{@came_from}"
		@came_from
	end

	private def pathfind_dijkstra(start, goal)
		frontier = PriorityQueue.new
		frontier << Element.new(start, 0)
		@came_from = {} of Point => Point?
		@came_from[start] = nil
		cost_so_far = {} of Point => Int32
		cost_so_far[start] = 0

		while !frontier.empty?
			current = frontier.pop.value
			break if current == goal
			log "current = #{current}"
			neighbors(*current).each do |nxt|
				new_cost = cost_so_far[current]# + cost(current, nxt)
				next if cost_so_far.has_key?(nxt) && new_cost >= cost_so_far[nxt]
				log "next = #{nxt}"
				cost_so_far[nxt] = new_cost
				frontier << Element.new(nxt, new_cost)
				@came_from[nxt] = current
			end
		end

		log "came_from = #{@came_from}"
		@came_from
	end
end

def reconstruct(came_from, start, goal)
	current = goal
	path = [current]
	while current != start
		break unless came_from.has_key? current
		current = came_from[current]
		path << current as Point
	end
	path << start
	path.reverse
end

def random_point(grid)
	{(rand * grid.cols).to_u as UInt32, (rand * grid.rows).to_u as UInt32}
end

def random_pathfind(grid)
	$start, $goal = random_point(grid), random_point(grid)
	grid.clear.fill
	grid.set(*$start, 1)
	grid.set(*$goal, 1)
	recalc_pathfind(grid)
end

def recalc_pathfind(grid)
	grid.path = reconstruct grid.pathfind($start, $goal, $algo), $start, $goal
end

######################### MAIN ############################

rows = 15_u32
cols = 15_u32
algorithms = [
	"breadth_first",
	"early_exit_breadth_first",
	"dijkstra"
]
$algo : String = algorithms[-1]

OptionParser.parse! do |parser|
	parser.banner = "Usage: #{$0} [args]"
	parser.on("-r ROWS", "--rows=ROWS", "Number of rows") { |r| rows = r.to_u32 }
	parser.on("-c COLS", "--cols=COLS", "Number of columns") { |c| cols = c.to_u32 }
	parser.on("-h", "--help", "Show this help") { puts parser; exit 1 }
	parser.on("-v", "--verbose", "Be verbose") { CONF[:verbose] = true }
	parser.on("-A", "--noarrows", "Don't show arrows") { CONF[:show_arrows] = false }
	parser.on("-w", "--weights", "Show weights") { CONF[:show_weights] = true }
	parser.on("-l", "--list", "List available algorithms") {
		puts "Algorithm index:"
		algorithms.each_with_index { |a, i| puts "#{i}  #{a}" }
		exit 0
	}
	parser.on("-a ALGO_NAME_OR_NUMBER", "--algorithm=ALGO_NAME_OR_NUMBER", "Algorithm") { |a| 
		begin
			$algo = algorithms[a.to_i]
		rescue ArgumentError
			$algo = a
		end
	}
end

$arrow : SF::Texture = SF::Texture.from_file("arrow.png") if CONF[:show_arrows]

# Create the grid
grid = Grid.new(rows, cols)
$start : Point = random_point(grid)
$goal : Point = random_point(grid)

# Create the rendering window
window = SF::RenderWindow.new(
	SF.video_mode(cols * TILE_SIZE, rows * TILE_SIZE),
	"Pathfind test")


random_pathfind grid

while window.open?
	while event = window.poll_event
		case event.type
		when SF::Event::KeyPressed
			case event.key.code
			when SF::Keyboard::Q
				window.close
			when SF::Keyboard::P
				random_pathfind grid
			when SF::Keyboard::A
				$algo = algorithms[(algorithms.index($algo) as Int + 1) % algorithms.size]
				recalc_pathfind grid
			end
		end
	end

	window.clear
	window.draw grid
	window.display
end