A*アルゴリズムで最短経路を探索してみます。
考え方は理論編の記事で紹介しています。
マス目クラスを定義する
今回はマス目上のマップを想定して最短経路を求めてみます。
何はともあれマス目がないと始まらないのでマス目を管理するクラスを作ります。
using System.Collections.Generic; using UnityEngine; [System.Serializable] public class AStarGrid { public enum GroundType { NotRoad, Road, } /// <summary> /// 座標 /// </summary> [System.Serializable] public struct Coord { public int x; public int y; public static Coord zero = new Coord(){ x = 0, y = 0 }; public static Coord one = new Coord(){ x = 1, y = 1 }; public static Coord left = new Coord(){ x = -1, y = 0 }; public static Coord up = new Coord(){ x = 0, y = 1 }; public static Coord right = new Coord(){ x = 1, y = 0 }; public static Coord down = new Coord(){ x = 0, y = -1 }; public static Coord operator +(Coord a, Coord b) { return new Coord(a.x + b.x, a.y + b.y); } public static Coord operator -(Coord a, Coord b) { return new Coord(a.x - b.x, a.y - b.y); } public float SqrMagnitude { get { return x * x + y * y; } } public float Magnitude { get { return Mathf.Sqrt(SqrMagnitude); } } public Coord(int x, int y) { this.x = x; this.y = x; } } /// <summary> /// セル /// </summary> [System.Serializable] public class Cell { public Coord coord; public GroundType groundType; } [SerializeField] private List<Cell> _cells; public List<Cell> Cells { get { return _cells; } } [SerializeField] private int _columnCount; public int ColumnCount { get { return _columnCount; } } [SerializeField] private int _rowCount; public int RowCount { get { return _rowCount; } } [SerializeField] private Coord _startCellCoord; public Cell StartCell { get { return GetCell(_startCellCoord); } set { _startCellCoord = value.coord; } } [SerializeField] private Coord _goalCellCoord; public Cell GoalCell { get { return GetCell(_goalCellCoord); } set { _goalCellCoord = value.coord; } } public AStarGrid(int columnCount, int rowCount) { _columnCount = columnCount; _rowCount = rowCount; _cells = new List<Cell>(); for (int i = 0; i < columnCount * rowCount; i++) { var column = Mathf.FloorToInt(i / rowCount); var row = i % rowCount; var coord = new Coord(){ x = column, y = row }; _cells.Add(new Cell(){ coord = coord }); } } /// <summary> /// セルを取得する /// </summary> public Cell GetCell(Coord coord){ return GetCell(coord.x, coord.y); } /// <summary> /// セルを取得する /// </summary> public Cell GetCell(int x, int y) { if (IsValidCoord(x, y)) { var index = x * _rowCount + y; return _cells[index]; } else { return null; } } /// <summary> /// 存在するセルか /// </summary> public bool IsValidCoord(int x, int y) { return x >= 0 && x < _columnCount && y >= 0 && y < _rowCount; } /// <summary> /// 隣接するセルを取得する /// </summary> public List<Cell> GetAdjacences(int x, int y) { var adjacences = new List<Cell>(); var offsets = new Coord[] { Coord.left, Coord.up, Coord.right, Coord.down }; for (int i = 0; i < offsets.Length; i++) { var cell = GetCell(x + offsets[i].x, y + offsets[i].y); if (cell != null) { adjacences.Add(cell); } } return adjacences; } }
AStarGridクラスはセルの個数分、Cellのインスタンスを保持します。
またセルは座標のほかにGroundTypeを持っています。
これがRoadのところのみ通れる（経路探索の対象となる）ようにする想定です。
公開するメソッドとしてはとりあえず当該座標のセルと隣接セルが取れれば良いかなという感じです。
A*計算クラスを定義する
次に、前節で作ったクラスを用いてA*を計算するクラスを作ります。
using System.Collections.Generic; using System.Linq; public class AStar { private class AStarInfo { public AStarGrid.Cell cell; public AStarInfo previous; public float step; public float distance; public float Weight { get { return step + distance; } } } private AStarGrid _grid; public AStar(AStarGrid grid) { _grid = grid; } /// <summary> /// 最短距離を取得する /// StartとGoalのセルを含む /// </summary> public List<AStarGrid.Cell> GetShortestWay(AStarGrid.Cell StartCell, AStarGrid.Cell GoalCell) { var res = new List<AStarGrid.Cell>(); var passedCells = new List<AStarGrid.Cell>(); var recentTargets = new List<AStarInfo>(); passedCells.Add(StartCell); recentTargets.Add(GetAStarInfo(StartCell, GoalCell, null)); AStarInfo goalInfo = null; while (true) { // recentTargetsのうちweightが最も低いものを計算対象とする var currentTarget = recentTargets .OrderBy(info => info.Weight) .FirstOrDefault(); // ターゲットの隣接セルのAStarInfoを取得する var adjacentInfos = _grid.GetAdjacences(currentTarget.cell.coord.x, currentTarget.cell.coord.y) .Where(cell => { // タイプが道でもなくゴールのセルでもない場合は対象外 if (cell.groundType != AStarGrid.GroundType.Road && cell != GoalCell) { return false; } // 計算済みのセルは対象外 if (passedCells.Contains(cell)) { return false; } return true; }) .Select(cell => GetAStarInfo(cell, GoalCell, currentTarget)) .ToList(); // recentTargetsとpassedCellsを更新 recentTargets.Remove(currentTarget); recentTargets.AddRange(adjacentInfos); passedCells.Add(currentTarget.cell); // ゴールが含まれていたらそこで終了 goalInfo = adjacentInfos.FirstOrDefault(info => info.cell == GoalCell); if (goalInfo != null) { break; } // recentTargetsがゼロだったら行き止まりなので終了 if (recentTargets.Count == 0) { break; } } // ゴールが結果に含まれていない場合は最短経路が見つからなかった if (goalInfo == null) { return res; } // Previousを辿ってセルのリストを作成する res.Add(goalInfo.cell); AStarInfo current = goalInfo; while(true){ if (current.previous != null) { res.Add(current.previous.cell); current = current.previous; } else { break; } } return res; } private AStarInfo GetAStarInfo(AStarGrid.Cell targetCell, AStarGrid.Cell goalCell, AStarInfo previousInfo) { var result = new AStarInfo(); result.cell = targetCell; result.previous = previousInfo; result.step = previousInfo == null ? 0 : previousInfo.step + 1; result.distance = (goalCell.coord - targetCell.coord).Magnitude; return result; } }
このクラスはAStarGridを参照して計算を行います。
GetShortestWay()にスタートとゴールのセルを与えると、最短経路のセル一覧を返してくれます。
上記のクラスを使うためのコードを書く
さてこれで準備は整いました。
あとはこれを使うコードを書きます。
#if UNITY_EDITOR using System.Collections.Generic; using UnityEngine; using System.Linq; using UnityEditor; [ExecuteInEditMode] public class AStarSample : MonoBehaviour { private const int COLUMN_COUNT = 10; private const int ROW_COUNT = 10; private const float CELL_SIZE = 1.0f; [SerializeField] private AStarGrid _grid = null; private AStar _aStar; private List<AStarGrid.Cell> _shortestWay = new List<AStarGrid.Cell>(); private void OnEnable() { if (_grid == null) { _grid = new AStarGrid(COLUMN_COUNT, ROW_COUNT); _grid.StartCell = _grid.GetCell(0, 0); _grid.GoalCell = _grid.GetCell(COLUMN_COUNT - 1, ROW_COUNT - 1); } _aStar = new AStar(_grid); } private void OnDrawGizmos() { Tools.current = Tool.None; var preColor = Gizmos.color; var preMatrix = Gizmos.matrix; Gizmos.matrix = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one * CELL_SIZE); // クリック判定 if(Event.current != null && Event.current.type == EventType.MouseUp){ // レイを取得する var clickedPosition = Event.current.mousePosition; clickedPosition.y = SceneView.currentDrawingSceneView.camera.pixelHeight - clickedPosition.y; var clickRay = SceneView.currentDrawingSceneView.camera.ScreenPointToRay(clickedPosition); // レイとy=0の平面との交点を求める var scale = Mathf.Abs(clickRay.origin.y / clickRay.direction.y); var intersection = clickRay.origin + clickRay.direction * scale; // 選択されたセルを編集するメニューを描画 intersection /= CELL_SIZE; var selectedColumn = Mathf.FloorToInt(intersection.x); var selectedRow = Mathf.FloorToInt(intersection.z); if (selectedColumn >= 0 && selectedColumn < COLUMN_COUNT && selectedRow >= 0 && selectedRow < ROW_COUNT) { GenericMenu menu = new GenericMenu(); // CellType変更メニュー var cell = _grid.GetCell(selectedColumn, selectedRow); menu.AddItem(new GUIContent ("Set Type > Not Road"), cell.groundType == AStarGrid.GroundType.NotRoad, () => { Undo.RecordObject(this, "Set Type > Not Road"); cell.groundType = AStarGrid.GroundType.NotRoad; EditorUtility.SetDirty(this); }); menu.AddItem(new GUIContent ("Set Type > Road"), cell.groundType == AStarGrid.GroundType.Road, () => { Undo.RecordObject(this, "Set Type > Road"); cell.groundType = AStarGrid.GroundType.Road; EditorUtility.SetDirty(this); }); menu.AddItem(new GUIContent ("Set Type > Start"), _grid.StartCell == cell, () => { Undo.RecordObject(this, "Set Type > Start"); _grid.StartCell = cell; EditorUtility.SetDirty(this); }); menu.AddItem(new GUIContent ("Set Type > Goal"), _grid.GoalCell == cell, () => { Undo.RecordObject(this, "Set Type > Goal"); _grid.GoalCell = cell; EditorUtility.SetDirty(this); }); menu.AddItem(new GUIContent ("Show Shortest Way"), false, () => { _shortestWay = _aStar.GetShortestWay(_grid.StartCell, _grid.GoalCell); }); menu.ShowAsContext(); } } // セルを描画 System.Action<AStarGrid.Cell> drawCell = cell => { Gizmos.DrawWireCube(new Vector3(cell.coord.x + 0.5f, 0.0f, cell.coord.y + 0.5f), new Vector3(1.0f, 0.0f, 1.0f)); }; Gizmos.color = Color.green; foreach (var item in _grid.Cells.Where(cell => cell.groundType == AStarGrid.GroundType.NotRoad)) { drawCell(item); } Gizmos.color = Color.yellow; foreach (var item in _grid.Cells.Where(cell => cell.groundType == AStarGrid.GroundType.Road)) { drawCell(item); } Gizmos.color = Color.blue; if (_grid.StartCell != null) { drawCell(_grid.StartCell); } Gizmos.color = Color.red; if (_grid.GoalCell != null) { drawCell(_grid.GoalCell); } Gizmos.color = Color.red; foreach (var item in _shortestWay) { Gizmos.DrawSphere(new Vector3(item.coord.x + 0.5f, 0.0f, item.coord.y + 0.5f), 0.2f); } Gizmos.color = preColor; Gizmos.matrix = preMatrix; } } #endif
なんか色々と書いてますが、次節の挙動を見てもらったほうが早いと思うので説明は割愛します。
使ってみる
前節のスクリプトを適当なGameObjectにアタッチすると、
ワールド座標の中心あたりにこのようなGizmoが表示されると思います。
これはAStarGridの情報を表示しているだけです。
今回はアルゴリズムの動作確認だけなので全部Gizmo描画です。
セルをクリックするとメニューが出てきて、道（Road）にしたり道じゃなくしたり、
Startセルに設定したりGoalセルに設定したりできます。
色々設定して下記のような感じに仕上がりました。
黄色が道、青がStart、赤がGoalです。
この状態でどこかをクリック > Show Shortest Wayを選択すると最短経路が表示されます。
Start地点をいろいろ変えて最短経路を探索してみます。
正常に探索できていそうなことが確認できました。
