ScalaでA* Algorithmで経路探索

scala

ふと気がつけば 函数プログラミングの集い 2011 in Tokyo - PARTAKE が開催されていたりしていて、数ヶ月前の状況を考えると「絶対参加できぬ... 変な期待は持たぬが吉...」みたいな感じだったのですが、今となってみると参加できたじゃん的な状況でUST観て見知ったる方々のご活躍を拝見しつつも近所のプール行ったり、学園祭行ったり、お祭り行ったりしてました。それはそれで楽しかったですけども。

で、思い立って何かプログラム書かないかんなぁ、という気持ちになったので久しぶりにA* Algorithmで経路探索をしてみたのです。

package com.komamitsu
import scala.collection.immutable.HashMap

case class Pos(val x: Int, val y: Int) {
  override def toString = {
    "x=" + x + ", y=" + y
  }

  def distance(other: Pos): Double = {
    math.sqrt(math.pow((x - other.x), 2) + math.pow((y - other.y), 2))
  }
}

class Node(val pos: Pos, val f: Double, val h: Double, val parent: Option[Node]) {
  override def toString = {
    "pos=" + pos + ", f=" + f + ", h=" + h + ", parent=" + parent
  }

  override def equals(that: Any) = that match {
    case other: Node => other.pos == pos
    case _ => false
  }
}

object Map {
  private val map = Array[String](
    "#############################",
    "#S                          #",
    "# ######################### #",
    "#                         # #",
    "# ### ##################### #",
    "# #                       # #",
    "# # ##################### # #",
    "# # #                   # # #",
    "# # # ################# # # #",
    "# ### #               # # # #",
    "# #   # # ########### # # # #",
    "# # # # #           # # # # #",
    "# # # # ########### # # # # #",
    "# # # # #         # # # # # #",
    "# # # # # ######### ### #####",
    "#   #   #           #      G#",
    "#############################")

  private val start: Pos = searchPos('S')
  private val goal: Pos = searchPos('G')
  private val dirs: List[Pos] = List(new Pos(0, -1), new Pos(0, 1), new Pos(-1, 0), new Pos(1, 0))

  private def isWalkable(pos: Pos): Boolean = {
    if (map(pos.y)(pos.x) != '#') true
    else false
  }

  private def searchPos(mark: Char): Pos = {
    var x = 0
    var y = 0
    for (line: String <- map) {
      x = 0
      for (char: Char <- line) {
        if (mark == char) { return (new Pos(x, y)) }
        x += 1
      }
      y += 1
    }
    throw new IllegalArgumentException("mark(" + mark + ") was not found")
  }

  def walk(): List[Node] = {
    def _walk(openXs: List[Node], closeXs: List[Node]): List[Node] = {

      if (openXs.isEmpty) {
        throw new IllegalArgumentException("no route to goal...")
      }

      val node: Node = openXs.min(Ordering[Double].on[Node](_.pos.distance(goal)))
      val openNodes: List[Node] = openXs.filterNot(_ == node)
      val closeNodes: List[Node] = node :: closeXs

      if (node.pos == goal) {
        return closeNodes
      }

      def challengeNewPos(openNodes: List[Node], closeNodes: List[Node], movDir: Pos): (List[Node], List[Node]) = {
        val newPos = new Pos(node.pos.x + movDir.x, node.pos.y + movDir.y)
        if (isWalkable(newPos)) {
          val cost = 1
          val hm = newPos.distance(goal)
          val fm = node.f - node.h + hm + cost

          val optFoundNodeInOpen: Option[Node] = openNodes.find(n => n.pos == newPos)
          val optFoundNodeInClose: Option[Node] = closeNodes.find(n => n.pos == newPos)

          optFoundNodeInOpen match {
            case Some(nodeInOpen) =>
              if (fm < nodeInOpen.f) {
                val newNodeInOpen = new Node(newPos, fm, hm, Some(node))
                (newNodeInOpen :: openNodes.filterNot(_ == nodeInOpen), closeNodes)
              } else {
                (openNodes, closeNodes)
              }
            case None =>
              optFoundNodeInClose match {
                case Some(nodeInClose) =>
                  if (fm < nodeInClose.f) {
                    val newNodeInOpen = new Node(newPos, fm, hm, Some(node))
                    (newNodeInOpen :: openNodes, closeNodes.filterNot(_ == nodeInClose))
                  } else {
                    (openNodes, closeNodes)
                  }
                case None =>
                  val newNodeInOpen = new Node(newPos, fm, hm, Some(node))
                  (newNodeInOpen :: openNodes, closeNodes)
              }
          }
        } else {
          (openNodes, closeNodes)
        }
      }

      val (newOpenNodes, newCloseNodes) =
        dirs.foldLeft((openNodes, closeNodes): (List[Node], List[Node]))(
          (nodesPair, newPos) => challengeNewPos(nodesPair._1, nodesPair._2, newPos))

      _walk(newOpenNodes, newCloseNodes)
    }

    val fOfStart = start.distance(goal)
    val openNodes = List(new Node(start, fOfStart, fOfStart, None))
    val closeNodes = List()
    _walk(openNodes, closeNodes)
  }

  def showRoute(closeNodes: List[Node]) {
    val nodeTable: HashMap[Pos, Node] =
      closeNodes.foldLeft(HashMap[Pos, Node]())(
        (map, node) =>
          node.parent match {
            case Some(p) => map + ((node.pos, p))
            case None => map
          })

    def createRouteFromGoal(current: Pos, route: List[Pos]): List[Pos] = {
      if (current == start) route.reverse
      else createRouteFromGoal(nodeTable(current).pos, current :: route)
    }

    val tempMap = map.clone()
    for (pos <- createRouteFromGoal(goal, List())) {
      val line = tempMap(pos.y)
      tempMap(pos.y) = line.substring(0, pos.x) + '+' + line.substring(pos.x + 1)
    }

    for (line <- tempMap) println(line)
  }
}

object AStar {
  def main(args: Array[String]) {
    val nodes = Map.walk()
    Map.showRoute(nodes)
  }
}

で、これを実行させると何だか迷路を解いている雰囲気が醸し出される。というか経路が一個しかないのでA*でやるありがたみが無いサンプルでgdgdな感じもありますけども。

#############################
#S                          #
#+######################### #
#+                        # #
#+### ##################### #
#+#                       # #
#+# ##################### # #
#+# #+++++++++++++++++++# # #
#+# #+#################+# # #
#+###+#               #+# # #
#+#+++# # ########### #+# # #
#+#+# # #           # #+# # #
#+#+# # ########### # #+# # #
#+#+# # #         # # #+# # #
#+#+# # # ######### ###+#####
#+++#   #           #  +++++#
#############################

で、ここ何回かScalaで書いてみて、OCamlとJavaの良いとこ取りという領域には全然たどり着けずに、むしろ何となく中途半端でできることとできないことの境界が不明瞭でその辺は「まともにScalaを覚えていない自分が悪い」という当然至極な感想を持っております。英語勉強の合間にちゃんと勉強せなあかん...

あと、個人的にはOCaml, Javaというより、Rubyに近しい雰囲気を感じ取ってしまっていて、そうすると「Rubyに比べると直感的では無いなぁ」という気もしていて、やはりその辺は「まともにScalaを覚えていない自分が悪い」という当然(ry