astar.rb

module Astar

  def sort_walking_neighbors(origin, routes)
    result = []
    routes.each_with_index do |r, i|
      result += r.map do |p|
        [i, p]
      end
    end
  
    result.sort! do |a, b|
      p_a, p_b = a[1], b[1]
      d_a, d_b = [p_a, p_b].map do |x|
        Math.sqrt((x[0] - origin[0]) ** 2 + (x[1] - origin[1]) ** 2)
      end
      
      if (d_a - d_b).abs < 1e-10
        if p_a[0] != p_b[0]
          p_a[0] <=> p_b[0]
        elsif p_a[1] != p_b[1]
          p_a[1] <=> p_b[1]
        else
          a[0][0] <=> b[0][0]
        end
      else
        d_a <=> d_b
      end
    end
  end
  
  def get_bus_neighbors(origin, routes)
    result = []
    routes.each_with_index do |s, j|
      k = s.index(origin)
      result << [j, s[k - 1]] if k && k > 0
      result << [j, s[k + 1]] if k && k < s.count - 1
    end
    result
  end

end

astar_test.rb

require 'test/unit'
require 'astar'

class AstarTestCase < Test::Unit::TestCase

  include Astar
  
  #   0 1 2 3 4
  # 0     -
  # 1     -
  # 2     -
  # 3 - - - - -
  # 4   * -
  def test_walking_neighbors
    route_1 = (0..4).map { |x| [3, x] }
    route_2 = (0..4).map { |x| [x, 2] }
    assert_equal([
        [0, [3, 1]],
        [1, [4, 2]],
        [0, [3, 0]],
        [0, [3, 2]],
        [1, [3, 2]],
        [1, [2, 2]],
        [0, [3, 3]],
        [1, [1, 2]],
        [0, [3, 4]],
        [1, [0, 2]]],
        sort_walking_neighbors([4, 1], [route_1, route_2]))
  end
  
  #   0 1 2 3 4
  # 0     -
  # 1     -
  # 2     -
  # 3 - - * - -
  # 4     -
  def test_get_bus_neighbors
    route_1 = (0..4).map { |x| [3, x] }
    route_2 = (0..4).map { |x| [x, 2] }
    assert_equal([
        [0, [3, 1]],
        [0, [3, 3]],
        [1, [2, 2]],
        [1, [4, 2]]],
        get_bus_neighbors([3, 2], [route_1, route_2]))
  end
  
end

test.rb

#!/usr/bin/env ruby

require 'fox16'
require 'yaml'
require 'test2'

include Fox

GRID_SIZE = 10
  
class ScribbleWindow < FXMainWindow

  def initialize(app)
    super(app, "Scribble Application", :width => 800, :height => 600)

    load_data
    @route_controls = []
    @mode = :draw
    
    @contents = FXHorizontalFrame.new(self,
        LAYOUT_SIDE_TOP | LAYOUT_FILL_X | LAYOUT_FILL_Y,
        :padLeft => 0, :padRight => 0, :padTop => 0, :padBottom => 0)

    @canvas_frame = FXVerticalFrame.new(@contents,
        LAYOUT_FILL_X | LAYOUT_FILL_Y | LAYOUT_TOP | LAYOUT_LEFT,
        :padLeft => 10, :padRight => 10, :padTop => 10, :padBottom => 10)

    @canvas = FXCanvas.new(@canvas_frame,
        :opts => LAYOUT_FILL_X | LAYOUT_FILL_Y | LAYOUT_TOP | LAYOUT_LEFT)
    @canvas.connect(SEL_PAINT) do |sender, sel, event|
      redraw_canvas
    end
    @canvas.connect(SEL_LEFTBUTTONPRESS) do |sender, sel, event|
      x, y = round_coordinate(event.win_x, event.win_y)
      case @mode
      when :set_start then
        @mode = :set_goal
        draw_start_goal_point(x, y, false)
        @data['start'] = [x, y]
      when :set_goal then
        @mode = :draw
        draw_start_goal_point(x, y, true)
        @data['goal'] = [x, y]
      else
        add_point(x, y)
        @data['routes'][@current_route_index]['points'] << [x, y]
      end
    end
    @canvas.connect(SEL_RIGHTBUTTONPRESS) do |sender, sel, event|
      points = @data['routes'][@current_route_index]['points']
      unless points.empty?
        points.delete_at(-1)
        redraw_canvas
        @canvas.update
      end
    end

    @control_frame = FXVerticalFrame.new(@contents,
        LAYOUT_FILL_Y | LAYOUT_TOP | LAYOUT_LEFT,
        :padLeft => 10, :padRight => 10, :padTop => 10, :padBottom => 10)

    clear_button = FXButton.new(@control_frame, "&Clear",
        :opts => FRAME_THICK | FRAME_RAISED | LAYOUT_FILL_X | LAYOUT_TOP | LAYOUT_LEFT)
    clear_button.connect(SEL_COMMAND) do
      @route_controls.each { |x| @control_frame.removeChild(x) }
      @control_frame.recalc
      @route_controls = []
      clear_data
      add_route_control(add_route)
      redraw_canvas
    end
    
    set_start_goal_button = FXButton.new(@control_frame, "Set start/&goal",
        :opts => FRAME_THICK | FRAME_RAISED | LAYOUT_FILL_X | LAYOUT_TOP | LAYOUT_LEFT)
    set_start_goal_button.connect(SEL_COMMAND) do
      @mode = :set_start
      @data['start'] = @data['goal'] = @astar_result = nil
      redraw_canvas
    end
    
    find_button = FXButton.new(@control_frame, "&Find!",
        :opts => FRAME_THICK | FRAME_RAISED | LAYOUT_FILL_X | LAYOUT_TOP | LAYOUT_LEFT)
    find_button.connect(SEL_COMMAND) do
      routes = @data['routes'].map { |x| x['points'] }
      
      a = RoutePoint.new(routes, @data['goal'], @data['start'][0], @data['start'][1], nil)
      b = RoutePoint.new(routes, @data['goal'], @data['goal'][0], @data['goal'][1], nil)
      @astar_result = astar(a, b)
      
      @canvas.update
    end
    
    clear_route_button = FXButton.new(@control_frame, "Clear &route",
        :opts => FRAME_THICK | FRAME_RAISED | LAYOUT_FILL_X | LAYOUT_TOP | LAYOUT_LEFT)
    clear_route_button.connect(SEL_COMMAND) do
      if @current_route_index
        @control_frame.removeChild(@route_controls[@current_route_index])
        @control_frame.recalc
        @data['routes'].delete_at(@current_route_index)
        @route_controls.delete_at(@current_route_index)
        if @data['routes'].empty?
          add_route_control(add_route)
        else
          @current_route_index = 0
        end
        redraw_canvas
      end
    end
    
    new_route_button = FXButton.new(@control_frame, "&New route",
        :opts => FRAME_THICK | FRAME_RAISED | LAYOUT_FILL_X | LAYOUT_TOP | LAYOUT_LEFT)
    new_route_button.connect(SEL_COMMAND) do
      add_route_control(add_route)
    end
    
    add_route if @current_route_index.nil?
    load_route_controls
    
    self.connect(SEL_CLOSE) do
      save_data
      0
    end
  end

  def create
    super
    show(PLACEMENT_SCREEN)
  end
  
  private
  def draw_grid(dc)
    dc.foreground = 'grey'
    (0..(@canvas.width / GRID_SIZE)).each do |x|
      dc.drawLine(x * GRID_SIZE, 0, x * GRID_SIZE, @canvas.height)
    end
    (0..(@canvas.height / GRID_SIZE)).each do |y|
      dc.drawLine(0, y * GRID_SIZE, @canvas.width, y * GRID_SIZE)
    end  
  end
  
  def draw_state(dc)
    @data['routes'].each do |route|
      dc.foreground = route['color']
      cur = nil
      route['points'].each do |p|
        draw_line(dc, cur[0], cur[1], p[0], p[1]) if cur
        draw_point(dc, p[0], p[1])
        cur = p
      end
    end
  end
  
  def draw_point(dc, x, y)
    dc.fillRectangle(x / GRID_SIZE * GRID_SIZE + 1,
        y / GRID_SIZE * GRID_SIZE + 1, GRID_SIZE - 1, GRID_SIZE - 1)
  end
  
  def draw_line(dc, x0, y0, x1, y1)
    dc.drawLine(x0, y0, x1, y1)
  end
  
  def draw_start_goal_point(x, y, goal = false)
    FXDCWindow.new(@canvas) do |dc|
      dc.foreground = @canvas.backColor
      dc.fillRectangle(x / GRID_SIZE * GRID_SIZE,
          y / GRID_SIZE * GRID_SIZE, GRID_SIZE, GRID_SIZE)

      w = dc.lineWidth
      dc.foreground = 'black'
      dc.lineWidth = 2
      dc.drawRectangle(x / GRID_SIZE * GRID_SIZE + 1,
          y / GRID_SIZE * GRID_SIZE + 1, GRID_SIZE - 1, GRID_SIZE - 1)
      dc.lineWidth = w

      if goal
        dc.fillRectangle(x / GRID_SIZE * GRID_SIZE + 3,
            y / GRID_SIZE * GRID_SIZE + 3, GRID_SIZE - 5, GRID_SIZE - 5)
      end
    end
  end
  
  def round_coordinate(x, y)
    return [x / GRID_SIZE * GRID_SIZE + GRID_SIZE / 2,
        y / GRID_SIZE * GRID_SIZE + GRID_SIZE / 2]
  end
  
  def redraw_canvas
    FXDCWindow.new(@canvas) do |dc|
      dc.foreground = @canvas.backColor
      dc.fillRectangle(0, 0, @canvas.width, @canvas.height)
      draw_grid(dc)
      draw_state(dc)

      if @astar_result
        dc.foreground = 'black'
        dc.lineStyle = LINE_ONOFF_DASH
        dc.lineWidth = 2
        cur = nil
        @astar_result.each do |p|
          dc.drawLine(cur.x, cur.y, p.x, p.y) if cur
          cur = p
        end
      end
    end
    
    draw_start_goal_point(@data['start'][0], @data['start'][1], false) if @data['start']
    draw_start_goal_point(@data['goal'][0], @data['goal'][1], true) if @data['goal']
  end
  
  def add_point(x, y)
    FXDCWindow.new(@canvas) do |dc|
      points = @data['routes'][@current_route_index]['points']
      dc.foreground = @data['routes'][@current_route_index]['color']
      draw_line(dc, points[-1][0], points[-1][1], x, y) unless points.empty?
      draw_point(dc, x, y)
    end
  end
  
  def load_data
    if File.exist?('state.yml')
      @data = YAML.load(File.read('state.yml'))
    else
      clear_data
    end
    @current_route_index = @data['current']

    nil
  end
  
  def save_data
    @data['current'] = @current_route_index
    File.open('state.yml', 'w+') { |x| x << @data.to_yaml }
    nil
  end
  
  def clear_data
    @data = { 'routes' => [], 'current' => nil }
    @current_route_index = nil
  end
  
  def add_route
    @data['routes'] << { 'points' => [], 'color' => 0xff000000 + rand(0x1000000) }
    result = @data['routes'][-1]
    @current_route_index = @data['routes'].index(result)
    result
  end
  
  def load_route_controls
    @data['routes'].each { |route| add_route_control(route) }
  end
  
  def add_route_control(route)
    color_well = FXColorWell.new(@control_frame, route['color'],
        :opts => LAYOUT_FILL_X | LAYOUT_TOP | LAYOUT_LEFT)
    color_well.connect(SEL_COMMAND) do |sender, sel, event|
      route['color'] = event
      redraw_canvas
    end
    color_well.connect(SEL_CLICKED) do |sender, sel, event|
      @current_route_index = @data['routes'].index(route)
    end
    color_well.create if self.created?
    @control_frame.recalc
    @route_controls[@data['routes'].index(route)] = color_well
    color_well
  end
  
end

class RoutePoint

  attr_reader :x, :y, :n

  def initialize(routes, goal, x, y, n)
    @routes, @goal, @x, @y, @n = routes, goal, x, y, n
  end
  
  def heuristic_cost_estimate(p)
    real_distance(p) / 4.0
  end
  
  def neighbor_nodes
    result = []
    
    unless @n.nil?
      r = @routes[@n]
      i = r.index([@x, @y])
      result << RoutePoint.new(@routes, @goal, r[i - 1][0], r[i - 1][1], @n) if i && i > 0
      result << RoutePoint.new(@routes, @goal, r[i + 1][0], r[i + 1][1], @n) if i && i < r.count - 1
      @routes.each_with_index do |s, o|
        if @n != o && s.index([@x, @y])
          result << RoutePoint.new(@routes, @goal, @x, @y, o)
        end
      end
    end
    
    @routes.each_with_index do |route, i|
      route.each do |p|
        if real_distance_a(p[0], p[1]) < GRID_SIZE * 10
          x = RoutePoint.new(@routes, @goal, p[0], p[1], i)
          unless result.index(x) || x == self
            result << x
          end
        end
      end
    end
    
    result << RoutePoint.new(@routes, @goal, @goal[0], @goal[1], nil)
    
    result
  end

  def distance_to(p)
    if @n.nil? || p.n.nil?
      real_distance(p)                  # andar a pé
    elsif @n == p.n && (route_index - p.route_index).abs <= 1
      real_distance(p) / 8.0            # andar uma estação
    else
      real_distance(p) + GRID_SIZE * 2  # trocar de linha
    end
  end
  
  def ==(other)
    @x == other.x && @y == other.y && @n == other.n
  end
  
  def eql?(other)
    @x == other.x && @y == other.y && @n == other.n
  end
  
  def to_s
    "#@n:[#@x,#@y]"
  end
  
  def route_index
    @routes[@n].index([@x, @y])
  end

  private
  def real_distance(p)
    real_distance_a(p.x, p.y)
  end

  def real_distance_a(x1, y1)
    Math.sqrt((@x - x1) ** 2 + (@y - y1) ** 2)
  end
  
end

if __FILE__ == $0
  application = FXApp.new('Scribble', 'FoxTest')
  scribble = ScribbleWindow.new(application)
  application.create
  application.run
end

test2.rb

class Point

  attr_reader :x, :y
  
  def initialize(x, y)
    @x, @y = x, y
  end
  
  def heuristic_cost_estimate(p)
    ((@x - p.x).abs + (@y - p.y).abs) * 1.01
  end
  
  def neighbor_nodes
    result = []
    result << Point.new(@x - 1, @y) if @x > 0
    result << Point.new(@x, @y - 1) if @y > 0
    result << Point.new(@x + 1, @y) if @x < 9
    result << Point.new(@x, @y + 1) if @y < 9
    result
  end

  def distance_to(p)
    (@x - p.x).abs + (@y - p.y).abs
  end
  
  def ==(other)
    @x == other.x && @y == other.y
  end
  
  def eql?(other)
    @x == other.x && @y == other.y
  end
  
  def to_s
    "[#@x, #@y]"
  end

end

def reconstruct_path(came_from, current_node)
  if c = get_hash_val(came_from, current_node)
    reconstruct_path(came_from, c) + [current_node]
  else
    [current_node]
  end
end

def get_hash_val(h, k)
  if x = h.assoc(k)
    x[1]
  end
end

def replace_hash_val(h, k, v)
  if x = h.assoc(k)
    h.delete(x[0])
  end
  h[k] = v
end

def astar(start, goal)
  closed_set = []
  open_set = [start]
  came_from = {}
  
  g_score = {}
  f_score = {}
  
  g_score[start] = 0
  f_score[start] = get_hash_val(g_score, start) + start.heuristic_cost_estimate(goal)
  
  while !open_set.empty? do
    puts "size(open_set) => #{open_set.count}; size(closed_set) => #{closed_set.count}"
    
    current = open_set.sort { |a, b| get_hash_val(f_score, a) <=> get_hash_val(f_score, b) }.first
    if current == goal
      result = reconstruct_path(came_from, goal)
      puts "result => #{result.inspect}"
      puts "g_score() => #{get_hash_val(g_score, current)}" 
      return result
    end
      
    open_set.delete(current)
    closed_set << current
    
    current.neighbor_nodes.each do |neighbor|
      next if closed_set.index(neighbor)
      
      tentative_g_score = get_hash_val(g_score, current) + current.distance_to(neighbor)
      
      if !open_set.index(neighbor) || tentative_g_score <= get_hash_val(g_score, neighbor)
        replace_hash_val(came_from, neighbor, current)
        replace_hash_val(g_score, neighbor, tentative_g_score)
        replace_hash_val(f_score, neighbor, get_hash_val(g_score, neighbor) + neighbor.heuristic_cost_estimate(goal))
        open_set << neighbor unless open_set.index(neighbor)
      end
    end
  end

  return nil
end

if __FILE__ == $0
  puts astar(Point.new(1, 1), Point.new(8, 8))
end