【C#/Unity】A*(A-Star)で経路探索を実装する

Unity の場合、3D, 2D ともに NavMesh とエージェントを使用すれば経路探索を自力で実装する機会はないかもしれませんが、経路探索をA*(A-Star)というアルゴリズムを自分で実装する場合の考え方と実装の紹介をしたいと思います。

グリッドベースの A* の経路探索は解説がいくつか見られるため、今回はノードベース (WayPoint) を用いた A* による経路探索を紹介したいと思います。

ちなみに、2D で NavMesh 以前記事を書いたのですが NavMesh の 2D 用を使用すると以下のリンク先の通り経路探索を利用することができます。

takap-tech.com

確認環境

  • Unity 2021.3
  • VisualStudio 2022
  • Windows11

Editor 上で動作を確認しています。

できること

通行可能なある地点を「WayPoint」(赤丸の個所)として、任意の位置に配置した WayPoint どうしを連結したものを経路情報として利用します。この WayPoint どうしの一連のつながりを A*(A-Star) という経路探索アルゴリズムで処理していきます。

実際に経路探索している様子は以下の通りです。

処理は、「初期化」 → 「経路探索を1Stepずつ進める(青文字)」 → 「求めたルートを表示(赤文字)」 → 「赤いルートに沿って緑のオブジェクトを移動」という順に進んでる様子が確認できると思います。

処理の説明

使用するデータ構造

まず以下のような WayPoint が網目状に接続されたグラフデータ構造を準備します。

正面から見ると平坦ですが、Z方向に高さを持つため横から見るとこのように高低差があります。NavMesh だとこの高低差がありすぎると経路として使用しない設定があると思いますが今回の実装では考慮していません。

考え方

見づらいので、以降は以下の通り、平面の図を使用していきます。カッコ内の図宇治は (X, Y, Z) の位置情報です。

まず Start と隣接するセルの各情報を調べて「Open(黄色)」状態とします。ノードは Open したときに以下の情報を記録します。そして Open済みのノードのうち最小の Score のノードを次のノードとして選択し(水色)、元の位置は「Close(灰色)」とします。

Total = 開始位置からそのノードまでの合計移動距離 Estimated = 終了位置からの直線距離 Score = Total + Estimated Parent = 直前のノード(表示していませんが関係を持たせています)

次のステップで選択したノードの隣接ノードのうち、まだ Open していないノードの情報を調べて「Open済み」とします。そして Open 済みのノードの中から最小 Score のノードを選択します。また、選択したノードはOpen対象から除外するので「Close」に変更します。

次のステップも同様の手順になります。Open済みのノードから最小 Score を選ぶ → 周囲を Open する → 元の位置を Close する、です。

そして、何度もこの処理を繰り返すと、Open したものの中に Goal のノードが現れます。この時点で処理を打ち切って探索を終了します。

最後に Goal から直前のノードを辿って Start までのノードを逆順に追うと Start → Goal の経路が得られます。

今回は狭い範囲で検索を行っていますが、実際はかなり広い領域に大量の WayPoint がある状態で処理を行うと思います。この時、遠すぎる位置を一度に検索するとかなり時間がかるため、ゲーム中では数フレームに分割したり、遠すぎる経路を探索から除外するなどの処理量を制限する仕組を別途設ける必要があります。

例えば、以下は考慮する必要があると思います。

  • ステップごとに処理を分割できるようにしておく
  • Total がある値より大きい場合経路判定から除外する
  • ある程度計算して到達できなかったら移動不能とする

実装コード

では実装を見ていきたいと思います。

使用するクラスは以下の通りです。

クラス名 説明
WayPoint 画面上で移動可能な地点を表すクラス
AStarNode WayPoint ごとに存在する経路探索中の情報を格納するクラス
AStar WayPointを使用した経路探索処理を行うクラス

WayPointクラス

移動可能な地点を表すUnityのコンポーネントです。GameObjectにアタッチして使用します。

隣あった移動可能なノードを「_relations」リストにインスペクターから設定してきます。

// WayPoint.cs

using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace Takap.Utility.Algorithm
{
    /// <summary>
    /// ゲーム中のある地点を表します。
    /// </summary>
    [ExecuteAlways]
    public partial class WayPoint : MonoBehaviour
    {
        // Inspector

        // 移動可能な隣接ノード
        [SerializeField] List<WayPoint> _relations;
        // 通行可能かどうか
        [SerializeField] bool _canMove = true;

        // Fields

        // 直前のノード隣接ノード状態を記録するリスト
        readonly List<WayPoint> _previousList = new List<WayPoint>();

        // Props

        // 移動可能な隣接ノードのリストを取得する
        public List<WayPoint> Rerations => _relations;

        // この地点が移動可能かどうかを取得します。
        public bool CanMove => _canMove;

        // Rintime impl

        private void Awake()
        {
            SynchronizeRelationsToPrevious();
            GizmoText = name;
        }

#if UNITY_EDITOR
        public void OnValidate()
        {
            if (_relations is null || _previousList is null)
            {
                return;
            }

            // 接続先の情報を更新する
            if (_relations.Count > _previousList.Count)
            {
                // 相手に自分を追加する(復路登録)
                foreach (var p in _relations.Except(_previousList))
                {
                    //Log.Trace(p.name);
                    if (!p.Rerations.Contains(this))
                    {
                        p.Rerations.Add(this);
                        p.SynchronizeRelationsToPrevious();
                    }
                }
            }
            else if (_relations.Count < _previousList.Count)
            {
                foreach (var p in _previousList.Except(_relations))
                {
                    p.Rerations.Remove(this); // 相手から自分を削除
                    p.SynchronizeRelationsToPrevious();
                }
            }

            SynchronizeRelationsToPrevious();

            GizmoText = name;
        }
#endif
        // Methods

        public void SynchronizeRelationsToPrevious()
        {
            _previousList.Clear();
            foreach (var p in _relations)
            {
                _previousList.Add(p);
            }
        }
    }
}

処理に関係ないデバッグ用の Gizmo 描画は以下のクラスに分割しています。

// WayPoint.Gizmo.cs

using UnityEngine;

#if UNITY_EDITOR
using UnityEditor;
#endif

namespace Takap.Utility.Algorithm
{
    // Gizmo描画処理
    public partial class WayPoint
    {
        // Gizmoで表示するテキスト
        public string GizmoText { get; set; }
        // Gizmoの線の色
        public Color GizmoColor { get; set; } = Color.white;

#if UNITY_EDITOR
        private void OnDrawGizmos()
        {
            GizmoDrawer.DispCrossMark(this, 0.2f, GizmoColor);
            Handles.Label(transform.localPosition, GizmoText);

            if (_relations is null || _relations.Count == 0)
            {
                return;
            }

            foreach (var next in _relations)
            {
                if (next is null) continue;

                // ポイント間に線を引く
                Color previous = Gizmos.color;
                if (GizmoText.Contains("Step") && next.GizmoText.Contains($"Step"))
                {
                    Gizmos.color = Color.red;
                }
                else if (GizmoText.Contains($"{NodeStatus.Open}")|| 
                           next.GizmoText.Contains($"{NodeStatus.Open}") &&
                         !(GizmoText.Contains($"{NodeStatus.None}") || 
                           next.GizmoText.Contains($"{NodeStatus.None}")))
                {
                    Gizmos.color = Color.yellow;
                }
                else if (GizmoText.Contains($"{NodeStatus.Close}") || 
                           next.GizmoText.Contains($"{NodeStatus.Close}"))
                {
                    Gizmos.color = Color.blue;
                }
                else if (GizmoText.Contains($"{NodeStatus.Exclude}") || 
                           next.GizmoText.Contains($"{NodeStatus.Exclude}"))
                {
                    Gizmos.color = Color.gray;
                }
                Gizmos.DrawLine(transform.localPosition, next.transform.localPosition);
                Gizmos.color = previous;
            }
        }
#endif
    }
}

冒頭のノードの配置で Point1 には以下のようにノードが設定されています。

このリストにインスペクター上で WayPoint を追加すると相手のノードのリストにも自分の情報が復路として自動で設定されます(一方通行の場合もあるかもしれませんがここでは考慮せず双方向としています)

AStarNodeクラス

経路探索を行っている最中の情報を記録するためのクラスです。単に情報を格納するだけのクラスです。

// AStarNode.cs

namespace Takap.Utility.Algorithm
{
    public class AStarNode
    {
        // 親ノード
        public AStarNode Parent { get; private set; }

        // 対応する位置
        public readonly WayPoint WayPoint; // Derived from Monobehaiour

        // ノードのステータス
        public NodeStatus Status { get; set; }

        // 開始地点からの総移動距離
        public float Total { get; private set; }

        // ゴールまでの推定移動距離
        public float Estimated { get; private set; }

        // ノードのスコア
        public float Score => Total + Estimated;

        // Constructors

        public AStarNode(WayPoint wayPoint)
        {
            WayPoint = wayPoint;
        }

        // Public Methods

        public void Open(AStarNode previous, AStarNode goal)
        {
            if (previous is not null)
            {
                Total = previous.Total + 
                    UnityEngine.Vector3.Distance(previous.WayPoint.transform.position, 
                        WayPoint.transform.position);
            }
            Estimated = UnityEngine.Vector3.Distance(WayPoint.transform.position, 
                goal.WayPoint.transform.position);

            Parent = previous;
            Status = NodeStatus.Open;
        }
    }

    public enum NodeStatus
    {
        None = 0,
        Open,
        Close,
        Exclude, // 探索対象にしない
    }
}

AStarWayPointクラス

WayPoint を AStarNode クラスに格納してノードの情報を使用してA*を使用して経路探索を行うためのクラスです。

コンストラクタで対象の WayPoint と開始位置、終了位置を指定した後、SearchOneStep メソッドの戻り値が SearchState.Completed になるまで繰り返しメソッドを呼び出します。探索する量が多くなると1フレーム内では終わらないので任意の位置で探索を打ち切って次のフレームで続きを実行していきます。

それが必要ない場合 SearchAll メソッドを呼び出して一気に経路探索を行います。

SearchState.Completed になったら GetRoute メソッドを呼び出して経路情報を配列として取得します。

// AStarWayPoint.cs

using System;
using System.Collections.Generic;
using System.Linq;

namespace Takap.Utility.Algorithm
{
    public class AStar
    {
        // Fields

        // オープン済みのノードを記憶するリスト
        readonly List<AStarNode> _openList = new();
        // 開始ノード
        public readonly AStarNode StartPoint;
        // 終了ノード
        public readonly AStarNode EndPoint;
        // 現在の検索処理状態
        SearchState _searchState;

        // Props

        // 探索対象のデータ管理
        public readonly Dictionary<WayPoint, AStarNode> NodeTable = new();

        // Constructors

        public AStar(IEnumerable<WayPoint> wayPoints, WayPoint startPoint, WayPoint endPoint)
        {
            foreach (var p in wayPoints)
            {
                if (p is null)
                {
                    continue;
                }

                AStarNode sn = new(p);
                if (!p.CanMove)
                {
                    sn.Status = NodeStatus.Exclude;
                }
                NodeTable[p] = sn;
            }

            StartPoint = NodeTable[startPoint];
            EndPoint = NodeTable[endPoint];

            if (!NodeTable.ContainsKey(startPoint))
            {
                throw new ArgumentException($"{nameof(wayPoints)}の中に" +
                    $"{nameof(startPoint)}が含まれていません。");
            }

            // 最初のタイルをOpenに設定する
            AStarNode node = NodeTable[startPoint];
            node.Open(null, NodeTable[endPoint]);
            _openList.Add(node);
        }

        // Public Methods

        // ルート検索を全て実行する
        public SearchState SearchAll()
        {
            SearchState state;
            while (true)
            {
                state = SearchOneStep();
                if (state != SearchState.Searching)
                {
                    break;
                }
            }

            return state;
        }

        // ルート検索を1ステップ実行する
        public SearchState SearchOneStep()
        {
            // 処理が完了している場合ステータスを返して検索処理しない
            if (_searchState != SearchState.Searching)
            {
                return _searchState;
            }

            AStarNode parentNode = GetMinCostNode();
            if (parentNode is null)
            {
                // 次にオープンするものが検索完了する前になくなった場合
                _searchState = SearchState.Incomplete;
                return _searchState;
            }

            // オープン済みリストからノードを削除して対象外にする
            parentNode.Status = NodeStatus.Close;
            _openList.Remove(parentNode);

            if (parentNode.WayPoint == EndPoint.WayPoint)
            {
                // 次に開いたノードがゴール地点だった
                _searchState = SearchState.Completed;
                return _searchState;
            }

            // 隣接ノードをすべてオープンする
            foreach (WayPoint aroundPoint in parentNode.WayPoint.Rerations)
            {
                if (!NodeTable.ContainsKey(aroundPoint))
                {
                    // コンストラクタで指定したポイントの中に隣接ノードが含まれていなかったらエラー
                    _searchState = SearchState.Error;
                    return _searchState;
                }

                AStarNode tmpNode = NodeTable[aroundPoint];
                if (tmpNode.Status != NodeStatus.None)
                {
                    continue;
                }

                tmpNode.Open(parentNode, EndPoint);

                _openList.Add(tmpNode);

                // 開いたノードが終点だったらその場で処理を打ち切って処理終了
                if (tmpNode.WayPoint == EndPoint.WayPoint)
                {
                    tmpNode.Status = NodeStatus.Close;
                    _openList.Remove(tmpNode);

                    _searchState = SearchState.Completed;
                    return _searchState;
                }
            }

            return _searchState;
        }

        // 検索結果のルートを取得する
        public WayPoint[] GetRoute()
        {
            if (_searchState != SearchState.Completed)
            {
                throw new InvalidOperationException("検索未完了ではルートを取得できません。");
            }

            AStarNode tmpNode = EndPoint;
            IEnumerable<WayPoint> f()
            {
                while (tmpNode.Parent != null)
                {
                    yield return tmpNode.WayPoint;
                    tmpNode = tmpNode.Parent;
                }

                yield return StartPoint.WayPoint;
            }

            var resultArray = f().ToArray();
            Array.Reverse(resultArray);
            return resultArray;
        }

        // オープン済みのノードリストから最小コストのノードを取得する 
        public AStarNode GetMinCostNode()
        {
            if (_openList.Count == 0)
            {
                return null;
            }

            // リストから最小コストのノードを選択する
            AStarNode minCostNode = _openList[0];
            if (_openList.Count > 1)
            {
                for (int i = 1; i < _openList.Count; i++)
                {
                    AStarNode tmpNode = _openList[i];
                    if (minCostNode.Score > tmpNode.Score)
                    {
                        minCostNode = tmpNode;
                    }
                }
            }
            return minCostNode;
        }
    }
}

テスト用のコード

参考程度ですが、テスト用のクラスです。以下のようにヒエラルキーを構成して Route にコンポーネントを配置します。

// AstarTest.cs

namespace Takap.Utility.Algorithm
{
    public class AstarTest : MonoBehaviour
    {
        [SerializeField] WayPoint _start; // 開始位置
        [SerializeField] WayPoint _end; // 終了位置
        [SerializeField] Transform _charactor;

        AStarWayPoint _astar;

        //[Button]
        public void StartAStar()
        {
            if (_start is null)
            {
                return;
            }

            if (_end is null)
            {
                return;
            }

            // 子要素のWayPointをすべて取得して経路情報に設定する
            _pointList = GetComponentsInChildren<WayPoint>();
            _astar = new AStarWayPoint(_pointList, _start, _end);
        }

        WayPoint[] _pointList;

        //[Button]
        public void StepNext()
        {
            SearchState status = _astar.SearchOneStep();
            Log.Trace($"AStar status={status}");

            UpdatePointsTextAndColor();
        }

        //[Button]
        public void ShowRute()
        {
            WayPoint[] routePoint = _astar.GetRoute();
            for (int i = 0; i < routePoint.Length; i++)
            {
                WayPoint p = routePoint[i];
                p.GizmoText = $"Step={i}\r\n{p.GizmoText}";
                p.GizmoColor = Color.red;
            }
        }

        //[Button]
        public async void MoveCharactor()
        {
            WayPoint[] routePoint = _astar.GetRoute();
            _charactor.transform.position = routePoint[0].transform.position;
            for (int i = 1; i < routePoint.Length; i++)
            {
                await _charactor.DOLocalMove(routePoint[i].transform.localPosition, 0.5f)
                    .SetEase(Ease.Linear).AsyncWaitForKill();
            }
        }

        private void UpdatePointsTextAndColor()
        {
            if (_pointList is null)
            {
                return;
            }

            foreach (AStarNode nodeInfo in _astar.NodeTable.Values)
            {
                string text = $"{nodeInfo.WayPoint.name}({nodeInfo.Status})\r\n"
                    + $"C={nodeInfo.C:F2}, H={nodeInfo.H:F2}";
                nodeInfo.WayPoint.GizmoText = text;

                switch (nodeInfo.Status)
                {
                    case NodeStatus.None: nodeInfo.WayPoint.GizmoColor = Color.white; break;
                    case NodeStatus.Open: nodeInfo.WayPoint.GizmoColor = Color.yellow; break;
                    case NodeStatus.Close: nodeInfo.WayPoint.GizmoColor = Color.blue; break;
                    case NodeStatus.Exclude: nodeInfo.WayPoint.GizmoColor = Color.gray; break;
                    default:
                        break;
                }
            }
        }
    }
}

これで経路網を設定した後 AStarWayPoint クラスの各メソッドを呼び出すと冒頭の経路探索ができるようになりました。

隣接ノードの選択の優先順位は、「自分と次の地点の距離 + 終点からの距離」が一番小さくなる順となっています。最小のノードを選択しくためきぞんの範囲からじわじわと検索済みが広がっていくような動作になります。途中で壁があったりすると検索量がはねあがったりします。

グリッドベースと違いノードが連結されていれば任意の位置同士を接続できるのが WayPoint ベースの検索の強みだと思います。