スタートノードからゴールノードまでのパスを計算する、このとき求めるパスが最短であることを保証しているアルゴリズム

a-star.scala

import scala.annotation.tailrec
import scala.collection._

object Main {
  def main(args: Array[String]) {

    // 1マスずつ進みながら、上下左右のマスでゴールに近い順に探索していく

    val map = """#S########
                |# ####   G
                |# #### ###
                |#  ###   #
                |##     ###""".stripMargin

    AStar(Mapper(map)).findShortestPath()
      .map(println)
  }

  case class Mapper(map: String) {
    val matrix: Array[Array[Vertex]] = map.lines.zipWithIndex.map { case (row, y) =>
      row.zipWithIndex.map { case (col, x) => Vertex(s"[${x}${y}]", col.toString, x, y) }.toArray
    }.toArray

    val start = matrix.flatMap(_.find(_.isStart)).apply(0)
    val goal = matrix.flatMap(_.find(_.isGoal)).apply(0)
  }

  case class AStar(mapper: Mapper) {

    type Path = Map[Vertex, Edge]
    private[this] val (matrix, start, goal) = (mapper.matrix, mapper.start, mapper.goal)
    private[this] val (ymax, xmax) = (matrix.size, matrix(0).size)

    def findShortestPath(): Seq[Vertex] = {
      @tailrec
      def fromGoal(e: Edge, r: Seq[Vertex] = Nil): Seq[Vertex] = e.from match {
        case Some(ef) => fromGoal(ef, e.v +: r)
        case None => e.v +: r
      }
      findPath().get(goal).map(fromGoal(_)).getOrElse(Nil) // goalからstartまで辿る
    }

    private[this] def findPath(): Path = {
      val q = mutable.PriorityQueue[Edge]()
      q += Edge(start, 0)

      @tailrec
      def find(path: Path = Map()): Path = {
        if (q.isEmpty) path
        else {
          val edge = q.dequeue()

          // 上下左右を見て移動可能先を小さい(近い)順に追加
          (for {
            (xd, yd) <- Seq((0, -1), (1, 0), (0, 1), (-1, 0));
            ne <- nextEdge(edge, xd, yd, goal) if path.get(ne.v).isEmpty
          } yield ne)
            .sortBy(_.weight)
            .foreach(q.enqueue(_))

          printMap(edge.v)
          val ne = path.get(edge.v).map(e => if (edge.weight < e.weight) edge else e).getOrElse(edge) // 訪問済みなら小さいほう優先
          val p = path + (edge.v -> ne)
          if (edge.v == goal) p else find(p)
        }
      }
      find(Map(start -> Edge(start, 0)))
    }

    private[this] def nextEdge(e: Edge, xd: Int, yd: Int, goal: Vertex): Option[Edge] = {
      def dist(v: Vertex): Double = math.abs((goal.x - v.x) + (goal.y - v.y))

      val (nx, ny) = (e.v.x + xd, e.v.y + yd)
      if ((nx >= 0 && nx < xmax) && (ny >= 0 && ny < ymax)) {
        val v = matrix(ny)(nx)
        if (v.isRoute) Some(Edge(v, weight + dist(v), Some(e))) else None
      } else None
    }

    private[this] def printMap(v: Vertex) {
      (0 until matrix.size).map { y =>
        (0 until matrix(y).size).map { x =>
          val vv = matrix(y)(x)
          if (v.x == vv.x && v.y == vv.y) print("●") else print(vv.mark)
        }
        println("")
      }
      println("")
    }
  }

  type Weight = Double
  val weight = 1.0d

  case class Graph(es: Map[Vertex, Seq[Edge]])
  case class Vertex(name: String, mark: String = "", x: Int = 0, y: Int = 0) {
    def isWall : Boolean = mark == "#"
    def isStart: Boolean = mark == "S"
    def isGoal : Boolean = mark == "G"
    def isRoute: Boolean = !isWall
  }
  case class Edge(v: Vertex, weight: Weight, from: Option[Edge] = None) extends Ordered[Edge] {
    def compare(e: Edge): Int = {
      if (e.weight < weight) -1
      else if (e.weight > weight) 1
      else 0
    }
  }
}