【Unity】幅優先探索で最短経路を求める

Unityで幅優先探索で最短経路を求めてみます。

Unity2019.1.10

幅優先探索？

この記事では幅優先探索により最短経路を求めてみます。
まずはこのアルゴリズムを理解します。

いま、このようにゴール地点と、通り抜けられない壁が存在するマス目があるとします。

幅優先探索ではまずゴールに隣接するマスに、自身を指す向きの矢印を書きます。

次に、先ほど矢印を書いたマスからさらに隣接するマスに、自身を指す向きの矢印を書きます。

この作業をマス目が埋まる（もしくは行き止まる）まで繰り返します。

あとは任意のマスを選択し、矢印を辿ればそれがゴールまでの最短距離となります。

幅優先探索のメリットとデメリット（A*アルゴリズムとの比較）

幅優先探索以外の経路探索アルゴリズムとして、当サイトでは過去にA*を紹介しています。

light11.hatenadiary.com

A*と比べると幅優先探索は、全部のセルを計算する必要があるものの、
ゴールが複数だったり任意のマスからの最短距離をまとめたりする場合にはメリットがあるといえそうです。

ソースコード

さてそれでは前節のアルゴリズムをソースコードで実装してみます。
まずは各マスごとの処理を書きます。

using UnityEngine;

public enum CellType {
    Road,
    Goal,
    Obstacle
}

/// <summary>
/// 経路探索のためのセル
/// </summary>
public class Cell : MonoBehaviour
{
    /// <summary>
    /// セルの種類
    /// </summary>
    public CellType Type { get; set; } = CellType.Road;
    /// <summary>
    /// 上のセル
    /// </summary>
    public Cell UpCell { get; set; }
    /// <summary>
    /// 右のセル
    /// </summary>
    public Cell RightCell { get; set; }
    /// <summary>
    /// 下のセル
    /// </summary>
    public Cell DownCell { get; set; }
    /// <summary>
    /// 左のセル
    /// </summary>
    public Cell LeftCell { get; set; }
    /// <summary>
    /// 進行方向に隣接したセル
    /// </summary>
    public Cell NextCell { get; set; }
    /// <summary>
    /// ゴールからのマス目数
    /// </summary>
    public int Distance { get; set; }

    /// <summary>
    /// 経路探索の準備
    /// </summary>
    public void PreparePathFinding () {
        Distance = Type == CellType.Goal ? 0 : int.MaxValue;
        NextCell = null;
    }

    /// <summary>
    /// 上方向に経路探索のパスを広げる
    /// </summary>
    public Cell ExtendPathToUp () => ExtendPathTo(UpCell);

    /// <summary>
    /// 右方向に経路探索のパスを広げる
    /// </summary>
    public Cell ExtendPathToRight () => ExtendPathTo(RightCell);

    /// <summary>
    /// 下方向に経路探索のパスを広げる
    /// </summary>
    public Cell ExtendPathToDown () => ExtendPathTo(DownCell);

    /// <summary>
    /// 左方向に経路探索のパスを広げる
    /// </summary>
    public Cell ExtendPathToLeft () => ExtendPathTo(LeftCell);

    private Cell ExtendPathTo(Cell neighbor) {
        if (neighbor == null || neighbor.Distance != int.MaxValue) {
            return null;
        }
        neighbor.Distance = Distance + 1;
        neighbor.NextCell = this;
        return neighbor.Type == CellType.Obstacle ? null : neighbor;
    }

    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(1, 0, 1));
        var gizmoColor = Gizmos.color;
        if (Type == CellType.Obstacle) {
            Gizmos.color = Color.red;
            DrawCross(transform.position, 0.5f);
        }
        else if (Type == CellType.Road) {
            if (NextCell != null) {
                Gizmos.color = Color.green;
                DrawArrow(transform.position, NextCell.transform.position - transform.position, 0.5f);
            }
        }
        else if (Type == CellType.Goal) {
            Gizmos.color = Color.blue;
            DrawRect(transform.position, 0.5f);
        }
        Gizmos.color = gizmoColor;
    }


    private void DrawArrow(Vector3 center, Vector3 direction, float scale)
    {
        var gizmoMatrix = Gizmos.matrix;
        Gizmos.matrix = Matrix4x4.TRS(center, Quaternion.LookRotation(direction, Vector3.up), Vector3.one * scale);
        var p1 = new Vector3(0, 0, 0.577f);
        var p2 = new Vector3(-0.5f, 0, -0.289f);
        var p3 = new Vector3(0.5f, 0, -0.289f);
        Gizmos.DrawLine(p1, p2);
        Gizmos.DrawLine(p2, p3);
        Gizmos.DrawLine(p3, p1);
        Gizmos.matrix = gizmoMatrix;
    }

    private void DrawCross(Vector3 center, float scale)
    {
        var gizmoMatrix = Gizmos.matrix;
        Gizmos.matrix = Matrix4x4.TRS(center, Quaternion.identity, Vector3.one * scale);
        var p1 = new Vector3(0.5f, 0, 0.5f);
        var p2 = new Vector3(0.5f, 0, -0.5f);
        var p3 = new Vector3(-0.5f, 0, -0.5f);
        var p4 = new Vector3(-0.5f, 0, 0.5f);
        Gizmos.DrawLine(p1, p3);
        Gizmos.DrawLine(p2, p4);
        Gizmos.matrix = gizmoMatrix;
    }

    private void DrawRect(Vector3 center, float scale)
    {
        var gizmoMatrix = Gizmos.matrix;
        Gizmos.matrix = Matrix4x4.TRS(center, Quaternion.identity, Vector3.one * scale);
        var p1 = new Vector3(0.5f, 0, 0.5f);
        var p2 = new Vector3(0.5f, 0, -0.5f);
        var p3 = new Vector3(-0.5f, 0, -0.5f);
        var p4 = new Vector3(-0.5f, 0, 0.5f);
        Gizmos.DrawLine(p1, p2);
        Gizmos.DrawLine(p2, p3);
        Gizmos.DrawLine(p3, p4);
        Gizmos.DrawLine(p4, p1);
        Gizmos.matrix = gizmoMatrix;
    }
}

上下左右のCellに対してExtendPathTo()を呼ぶことで最短経路の進行方向のセルがNextCellとして入ってきます。
下の方はギズモ描画用のメソッドです。

次にこのCellを管理するクラスを実装します。

using System.Collections.Generic;
using UnityEngine;

public class Field : MonoBehaviour
{
    /// <summary>
    /// CellをアタッチしたPrefab
    /// </summary>
    [SerializeField]
    private Cell _cellPrefab = default;
    /// <summary>
    /// マス目の下図
    /// </summary>
    [SerializeField]
    private Vector2Int _size;
    /// <summary>
    /// ゴールの座標
    /// </summary>
    [SerializeField]
    private Vector2Int[] _goalCoords;
    /// <summary>
    /// 障害物の座標
    /// </summary>
    [SerializeField]
    private Vector2Int[] _obstacleCoords;

    private Cell[,] _cells;

    private void Start () {
        // セルを生成
        _cells = new Cell[_size.x, _size.y];
        for (int i = 0; i < _size.x; i++) {
            for (int j = 0; j < _size.y; j++) {
                var cell = _cells[i, j] = Instantiate(_cellPrefab);
                cell.transform.SetParent(transform, false);
                cell.transform.localPosition = new Vector3(i, 0, j);
                cell.Type = CellType.Road;
                if (i >= 1) {
                    cell.LeftCell = _cells[i-1, j];
                    _cells[i-1, j].RightCell = cell;
                }
                if (j >= 1) {
                    cell.DownCell = _cells[i, j-1];
                    _cells[i, j-1].UpCell = cell;
                }
            }
        }

        // ゴールと障害物を設定
        for (int i = 0; i < _goalCoords.Length; i++) {
            var coord = _goalCoords[i];
            _cells[coord.x, coord.y].Type = CellType.Goal;
        }
        for (int i = 0; i < _obstacleCoords.Length; i++) {
            var coord = _obstacleCoords[i];
            _cells[coord.x, coord.y].Type = CellType.Obstacle;
        }

        // 経路探索の準備
        var targetCells = new Queue<Cell>();
        for (int i = 0; i < _cells.GetLength(0); i++) {
            for (int j = 0; j < _cells.GetLength(1); j++) {
                var cell = _cells[i, j];
                cell.PreparePathFinding();
                if (cell.Type == CellType.Goal) {
                    targetCells.Enqueue(cell);
                }
            }
        }

        // 幅優先で経路探索
        while (targetCells.Count > 0) {
            var cell = targetCells.Dequeue();
            if (cell != null) {
                var upCell = cell.ExtendPathToUp();
                if (upCell != null) targetCells.Enqueue(upCell);
                var downCelll = cell.ExtendPathToDown();
                if (downCelll != null) targetCells.Enqueue(downCelll);
                var rightCell = cell.ExtendPathToRight();
                if (rightCell != null) targetCells.Enqueue(rightCell);
                var leftCell = cell.ExtendPathToLeft();
                if (leftCell != null) targetCells.Enqueue(leftCell);
            }
        }
    }
}

_cellPrefabには先ほどのCellコンポーネントをアタッチしたPrefabを設定します。
すると_sizeに入力した大きさのマス目が生成されます。
ゴールと障害物の座標は_goalCoordsと_obstacleCoordsに入力しておきます。

再生結果

Fieldを適当なGame Objectにアタッチしてから再生すると、Sceneビューに次のようなGizmoが表示されます。

青い四角はゴールとして設定したマス、赤い×は障害物として設定したセルです。
三角形は進行方向を示しています。
上図から、ゴールへの最短経路が正常に求められていることがわかりました。

関連

light11.hatenadiary.com