Last active
January 2, 2016 16:29
-
-
Save ryoppy/8330934 to your computer and use it in GitHub Desktop.
スタートノードからゴールノードまでのパスを計算する、このとき求めるパスが最短であることを保証しているアルゴリズム
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment