messing with pathfinding algorithms

pathfind.rb

require 'pqueue'
require 'set'

### class Point and subclasses

Point = Struct.new(:x, :y)

class Point

  attr_reader :marked

  def dist(p2)
    (x - p2.x).abs + (y - p2.y).abs
  end

  def ==(p2)
    x == p2.x && y == p2.y
  end

  alias :eql? :==

  def hash
    [x, y].hash
  end

  def neighbors
    [-1, 1].map { |xinc|
      Point.new(x + xinc, y)
    } + [-1, 1].map { |yinc|
      Point.new(x, y + yinc)
    }.flatten
  end

  def mark; @marked = true; end
  def clear; @marked = false; end
end

class Target < Point; end
class Path < Point; end

### class Map

class Map

  attr_reader :width, :height, :target

  def initialize(width, height)
    @width = width
    @height = height
    @map = Hash.new do |h, k|
      h[k] = if (1..@width).include?(k.x) && (1..@height).include?(k.y)
               k
             else
               nil
             end
    end
    random_fill
  end

  def set_target(x, y)
    @map[@target] = nil
    target = Target.new(x, y)
    @target = @map[target] = target
  end

  QueuedNode = Struct.new(:node, :score)

  def find_path2(start_x, start_y)
    start_node = get_at(start_x, start_y)
    distances  = { start_node => 0.0 }

    open_set  = PQueue.new{ |a, b| a.score < b.score }
    opened    = Set.new
    score     = lambda { |n| distances[n] + n.dist(@target) }
    push_node = lambda { |n| open_set.push(QueuedNode.new(n, score[n])) }
    pop_node  = lambda { open_set.pop.node }
    push_node[start_node]

    until open_set.empty?
      current_node = pop_node[]

      # heuristic check below can cause this to happen
      next if current_node.marked
      current_node.mark

      current_node.neighbors.each do |_n|
        n = get_at(_n.x, _n.y)
        next if n.nil? || n.marked

        # update the minimum distance
        new_dist = distances[current_node] + 1
        if distances[n]
          distances[n] = [new_dist, distances[n]].min
        else
          distances[n] = new_dist
        end

        # if our heuristic says n is no good
        if distances[@target] && score[n] > score[current_node]
          n.mark
        elsif !opened.include?(n)
          push_node[n] 
          opened.add(n)
        end
      end
    end

    @map.each { |_, n| n.clear if n.is_a?(Point) }
    return false unless distances[@target]

    # trace path back to start
    path = [@target]
    until distances[path.last] == 0.0
      neighbors = path.last.neighbors
      dst = neighbors.zip(neighbors.map { |n| distances[n] })
      dst.delete_if { |d| d.last.nil? }

      next_node = dst.sort_by(&:last).first.first
      @map[next_node] = Path.new(next_node.x, next_node.y)
      path << next_node
    end

    true
  end

  def inspect
    lines = []
    lines << (['  '] + 1.upto(width).to_a.map { |n| sprintf('%2d', n) }).join(' ')
    1.upto(@height) do |yi|
      line = [sprintf('%2d', yi)]
      1.upto(@width) do |xi|
        line << case @map[Point.new(xi, yi)]
                when Target
                  'XX'
                when Path
                  '**'
                when Point # above two are subclasses of Point
                  '..'
                else
                  'HH'
                end

      end
      lines << line.join(' ')
    end
    lines.join(?\n)
  end

  private

  def get_at(x, y)
    @map[Point.new(x, y)]
  end

  def random_fill(area_fraction: 0.5, max_dim: (@width + @height) / 20.0)
    total_area = @width * @height
    filled_area = 0
    get_range = lambda do |l|
      start = rand(1..l)
      start..rand(start..start + max_dim)
    end
    while filled_area < total_area * area_fraction
      x_range = get_range[@width]
      y_range = get_range[@height]
      x_range.each do |xi|
        y_range.each do |yi|
          point = Point.new(xi, yi)
          if @map[point]
            @map[point] = nil
            filled_area += 1
          end
        end
      end
    end
  end

end

width, height = *ARGV

map = Map.new(width.to_i, height.to_i)
puts map.inspect

print '> '

while (cmd = $stdin.gets.strip) != 'q'
  case cmd
  when ?n
    map = Map.new(width.to_i, height.to_i)
  when /t (\d+) (\d+)/
    map.set_target($1.to_i, $2.to_i)
  when /p (\d+) (\d+)/
    t = Time.now
    map.find_path2($1.to_i, $2.to_i)
    puts "Time to solve: #{Time.now - t} seconds"
  end
  puts map.inspect
  print '> '
end