Help us understand the problem. What is going on with this article?

C#で経路探査アルゴリズムを作ってみた

完成図

446a81c014ab63b36030a7d4dcf371a6.png

はじめに

映画「メイズ・ランナー」を見ていたら、ふと経路探査アルゴリズムを作りたくなってしまいました。

2Dでブロックを描画できればよかったので、ぱっと思い浮かんだのはUnityエンジンでした。
しかし、
ab1630ae061f9592637996d50e095d3e.png
ご覧の通り、僕のPCにはもうUnityが入る隙がありませんでした。
そこで、致し方なくWinFormに描画することにしました。

描画方法

具体的には、WinFormにPictureBoxを配置してBitmapに描きます。

pictureBox1.Image = World.bitmap;

座標管理

今回は2Dで描画するので、各タイルのとりえる座標は x, y です。
これをより扱いやすくする為に2次元ベクトルとしてクラス化しました。

また、例えば2つのベクトルを計算したい時に、

Vector2 v1 = new Vector2(1, 1);
Vector2 v2 = new Vector2(1, 1);
v1.x = v1.x + v2.x;
v1.y = v1.y + v2.y;

とハードコーディングするのはしんどいですし、ミスも誘発しそうです。
ということで、演算子をオーバーロードしました。

Vector2 result = new Vector2();
result = v1 + v2;

完全なコードはGithubにもあります。
Github | Vector2.cs

class Vector2
    {
        ////////////////////////////////////////////////////////////////////////
        // 2次元ベクトルにおける X座標とY座標を保持
        ////////////////////////////////////////////////////////////////////////
        public int x { get; set; }
        public int y { get; set; }

        ////////////////////////////////////////////////////////////////////////
        // コンストラクタ
        ////////////////////////////////////////////////////////////////////////
        public Vector2(int _x_, int _y_)
        {
            this.x = _x_;
            this.y = _y_;
        }

        ////////////////////////////////////////////////////////////////////////
        // 加法演算子のオーバーロード
        ////////////////////////////////////////////////////////////////////////
        public static Vector2 operator +(Vector2 n1, Vector2 n2)
        {
            return new Vector2(n1.x + n2.x, n1.y + n2.y);
        }

        ////////////////////////////////////////////////////////////////////////
        // 減法演算子のオーバーロード
        ////////////////////////////////////////////////////////////////////////
        public static Vector2 operator -(Vector2 n1, Vector2 n2)
        {
            return new Vector2(n1.x - n2.x, n1.y - n2.y);
        }

        ////////////////////////////////////////////////////////////////////////
        // 乗算演算子のオーバーロード
        ////////////////////////////////////////////////////////////////////////
        public static Vector2 operator *(Vector2 n1, Vector2 n2)
        {
            return new Vector2(n1.x * n2.x, n1.y * n2.y);
        }

        ////////////////////////////////////////////////////////////////////////
        // 2点の座標における距離を整数で取得
        ////////////////////////////////////////////////////////////////////////
        public static int Distance(Vector2 v1, Vector2 v2)
        {
            return (int)Math.Sqrt(Math.Pow(v1.x - v2.x, 2) + Math.Pow(v1.y - v2.y, 2));
        }

        ////////////////////////////////////////////////////////////////////////
        // 2点の座標における距離を小数点で取得
        ////////////////////////////////////////////////////////////////////////
        public static double DistanceEx(Vector2 v1, Vector2 v2)
        {
            return Math.Sqrt(Math.Pow(v1.x - v2.x, 2) + Math.Pow(v1.y - v2.y, 2));
        }
    }

ワールド管理

思ったよりソースコードが巨大になってしまったので、簡易的に書きます。
完全なコード: Github | World.cs

タイルの保持

タイルは配列に保持しておきます。

private static TileBlock[,] tileBlocks { get; set; }

イニシャライズ

1.コンテキストの作成

public static void CreateContext()
{
    graphics = Graphics.FromImage(GetBitmap());
    if(graphics != null)
    {
        LoggerForm.WriteSuccess("Context created.");
    }
    else
    {
        LoggerForm.WriteError("CreateContext() failed.");
    }
}

2.マップの生成

タイルを保持するための配列のメモリ確保と
ビットマップ上におけるタイルサイズの定義を行います。
そして、空のタイルを敷き詰めておきます。

public static void CreateMap()
{
    tileBlocks = new TileBlock[MAX_COORD_X, MAX_COORD_Y];

    tileSizeX = (bitmap.Width) / MAX_COORD_X;
    tileSizeY = (bitmap.Height) / MAX_COORD_Y;

    for (int x = 0; x < MAX_COORD_X; x++)
    {
        for (int y = 0; y < MAX_COORD_Y; y++)
        {
            AddTile(new Vector2(x, y), TileType.Walkable);
        }
    }
    LoggerForm.WriteSuccess("Map created.");
}

タイルの管理

タイルには座標だけでなく探索する際に使用するデータやその他色々な値を格納しておきたかったので、
構造体と挙列型を用意しました。

完全なコード: GitHub | TileBlock.cs

データ構造:
Untitled Diagram (2).png

////////////////////////////////////////////////////////////////////////
// タイル属性を示すEnum
////////////////////////////////////////////////////////////////////////
public enum TileType : int
{
    Walkable        = 1,
    Wall            = 2,
    StartTile       = 3,
    GoalTile        = 4,
    NullTile        = 5,
    AnalyzedTile    = 6
}
////////////////////////////////////////////////////////////////////////
// 探索に使用する属性を示すEnum
////////////////////////////////////////////////////////////////////////
public enum AnalyzeAttributes : int
{
    INull       = 0x00000000,
    IOpenedTile = 0x00000030,
    IClosedTile = 0x00000040,
}
////////////////////////////////////////////////////////////////////////
// 探索データ構造体
////////////////////////////////////////////////////////////////////////
public struct AnalyzeData
{
    public int コスト;
    public int 推定コスト;
    public int スコア;
}
////////////////////////////////////////////////////////////////////////
// タイル構造体
////////////////////////////////////////////////////////////////////////
private struct TileStruct
{
    public Vector2              coordinate;
    public TileType             tileType;
    public AnalyzeAttributes    attributes;
    public AnalyzeData          analyzeData;
    public bool                 isAnalyzed;
}

描画関連

タイルの描画位置

まず初めに、今回使用するビットマップのサイズ$BS$は360×360です。
また、タイルの最大座標$MS$は25×25です。

このときビットマップ上に於けるタイルの縦/横描画サイズ$S$は

S(x,y) = \frac{BS}{MS}

マップの周りの余白$w$は

w = \frac{BS-S(x,y)}{2}

タイルの位置(左上角)$L$、余白$w$は

L = S(x,y)+w

です。
これを関数で表すと

tileSizeX = bitmap.Width / MAX_COORD_X;
tileSizeY = bitmap.Height / MAX_COORD_Y;

public static Size GetTileSize()
{
    return new Size(tileSizeX, tileSizeY);
}

public static Vector2 GetWhiteSpace()
{
    var x = (bitmap.Width  - (GetTileSize().Width  * MAX_COORD_X)) / 2;
    var y = (bitmap.Height - (GetTileSize().Height * MAX_COORD_Y)) / 2;
    return new Vector2(x, y);
}

public static Point GetRednerTileLocation(Vector2 coord)
{
    Vector2 drawCoord = new Vector2(tileSizeX * coord.x, tileSizeY * coord.y);
    return new Point(drawCoord.x + GetWhiteSpace().x, drawCoord.y + GetWhiteSpace().y);
}

となります。

マウス座標からタイル座標を取得

ビットマップ上のマウス座標x,yからタイル座標x,yを算出します。

1x8a9-qoncl.gif

結果のタイル座標$cx,cy$は

\begin{align}
ax = x+w\\
ay = y+h
\end{align}
cxのとりえる値の範囲は x \leqq cx \leqq ax にあるタイル\\&&\\
cyのとりえる値の範囲は y \leqq cy \leqq ay にあるタイル

であると言えます。
これを関数で表すと

public static Vector2 GetTileCoordByMouseCoord(Vector2 coord)
{
    for(int x = 0; x < MAX_COORD_X; x++)
    {
        for(int y = 0; y < MAX_COORD_Y; y++)
        {
            var tileLocation = GetRednerTileLocation(new Vector2(x, y));
            var tileSize = GetTileSize();

            var xX = tileLocation.X;
            var Xx = tileLocation.X + tileSize.Width;

            var yY = tileLocation.Y;
            var Yy = tileLocation.Y + tileSize.Height;

            if(xX <= coord.x && Xx >= coord.x) // x ≦ cx ≦ax
            {
                if(yY <= coord.y && Yy >= coord.y)// y ≦ cy ≦ay
                {
                    return new Vector2(x, y);
                }
            }
        }
    }
    return new Vector2(0, 0);
}

となります。
コッチの方が美しいですね

if(xX <= coord.x && coord.x <= Xx) // x ≦ cx ≦ax
{
    if(yY <= coord.y && coord.y <= Yy)// y ≦ cy ≦ay
    {
        return new Vector2(x, y);
    }
}

アルゴリズム

スタートからゴールへの方角を算出

////////////////////////////////////////////////////////////////////////
// 方角を示すEnum
////////////////////////////////////////////////////////////////////////
public enum ArrowVector
{
    NULL        ,

    UP          ,
    DOWN        ,
    RIGHT       ,
    LEFT        ,

    UP_RIGHT    ,
    UP_LEFT     ,

    DOWN_RIGHT  ,
    DOWN_LEFT   ,
}

public static ArrowVector CalculateVector(Vector2 start, Vector2 goal)
{
    if(start.x == goal.x)
    {
        if(start.y > goal.y)
        {
            return ArrowVector.UP;
        }
        else
        {
            return ArrowVector.DOWN;
        }
    }

    if(start.y == goal.y)
    {
        if(start.x > goal.x)
        {
            return ArrowVector.LEFT;
        }
        else
        {
            return ArrowVector.RIGHT;
        }
    }

    if (start.x > goal.x && start.y > goal.y) return ArrowVector.UP_LEFT;
    if (start.x < goal.x && start.y > goal.y) return ArrowVector.UP_RIGHT;

    if (start.x < goal.x && start.y < goal.y) return ArrowVector.DOWN_RIGHT;
    if (start.x > goal.x && start.y < goal.y) return ArrowVector.DOWN_LEFT;

    return ArrowVector.NULL;
}

進む方向を決める

可読性を求めていたらいつの間にかハードコーディングしてました。

データ処理フロー:
Untitled Diagram-Page-2.png

////////////////////////////////////////////////////////////////////////
// 4方向から進むに最適なタイルを算出
////////////////////////////////////////////////////////////////////////
private static TileBlock GetBestTile(TileBlock origin, int cost)
{
    if (origin.GetTileType() == TileType.GoalTile || origin.GetAnalyzed())
    {
        return origin;
    }

    var goalCoord   = GetTileBlockByTileType(TileType.GoalTile).GetCoordinate();
    var originCoord = origin.GetCoordinate();

    var up      = GetTileBlock(new Vector2(originCoord.x, originCoord.y - 1));
    var bottom  = GetTileBlock(new Vector2(originCoord.x, originCoord.y + 1));
    var right   = GetTileBlock(new Vector2(originCoord.x + 1, originCoord.y));
    var left    = GetTileBlock(new Vector2(originCoord.x - 1, originCoord.y));

    //ふるいにかける
    if (up      != null && up.      GetTileType() != TileType.Walkable && up.GetTileType()      != TileType.GoalTile) up      = null;
    if (bottom  != null && bottom.  GetTileType() != TileType.Walkable && bottom.GetTileType()  != TileType.GoalTile) bottom  = null;
    if (right   != null && right.   GetTileType() != TileType.Walkable && right.GetTileType()   != TileType.GoalTile) right   = null;
    if (left    != null && left.    GetTileType() != TileType.Walkable && left.GetTileType()    != TileType.GoalTile) left    = null;

    //どれかが前のoriginだったらやめる
    if (up      != null && up.      GetAnalyzed())  up     = null;
    if (bottom  != null && bottom.  GetAnalyzed())  bottom = null;
    if (right   != null && right.   GetAnalyzed())  right  = null;
    if (left    != null && left.    GetAnalyzed())  left   = null;

    //どれかがゴールだったらそこまで線を描画
    if (up      != null && up.      GetTileType() == TileType.GoalTile) DrawLineCenterTileToTile(origin.GetCoordinate(), up.    GetCoordinate());
    if (bottom  != null && bottom.  GetTileType() == TileType.GoalTile) DrawLineCenterTileToTile(origin.GetCoordinate(), bottom.GetCoordinate());
    if (right   != null && right.   GetTileType() == TileType.GoalTile) DrawLineCenterTileToTile(origin.GetCoordinate(), right. GetCoordinate());
    if (left    != null && left.    GetTileType() == TileType.GoalTile) DrawLineCenterTileToTile(origin.GetCoordinate(), left.  GetCoordinate());

    var up_hcost        = 0;
    var bottom_hcost    = 0;
    var right_hcost     = 0;
    var left_hcost      = 0;

    //推定コストを計算
    if (up      != null)    up_hcost        = CalculateHeuristic(up.    GetCoordinate(), goalCoord);
    if (bottom  != null)    bottom_hcost    = CalculateHeuristic(bottom.GetCoordinate(), goalCoord);
    if (right   != null)    right_hcost     = CalculateHeuristic(right. GetCoordinate(), goalCoord);
    if (left    != null)    left_hcost      = CalculateHeuristic(left.  GetCoordinate(), goalCoord);

    //データをセット
    if (up      != null)    up.     SetAnalyzeData(     cost,   up_hcost       );
    if (bottom  != null)    bottom. SetAnalyzeData(     cost,   bottom_hcost   );
    if (right   != null)    right.  SetAnalyzeData(     cost,   right_hcost    );
    if (left    != null)    left.   SetAnalyzeData(     cost,   left_hcost     );

    var up_score        = 0;
    var bottom_score    = 0;
    var right_score     = 0;
    var left_score      = 0;

    if (up      != null)    up_score        = up.       GetScore();
    if (bottom  != null)    bottom_score    = bottom.   GetScore();
    if (right   != null)    right_score     = right.    GetScore();
    if (left    != null)    left_score      = left.     GetScore();

    var scores  = new int[4]        ;
    scores[0]   = up_score          ;
    scores[1]   = bottom_score      ;
    scores[2]   = right_score       ;
    scores[3]   = left_score        ;

    var hcosts  = new int[4]        ;
    hcosts[0]   = up_hcost          ;
    hcosts[1]   = bottom_hcost      ;
    hcosts[2]   = right_hcost       ;
    hcosts[3]   = left_hcost        ;

    var tiles   = new TileBlock[4]  ;
    tiles[0]    = up                ;
    tiles[1]    = bottom            ;
    tiles[2]    = right             ;
    tiles[3]    = left              ;

    var min_score   = int.MaxValue  ;
    var min_cost    = int.MaxValue  ;
    var min_hcost   = int.MaxValue  ;
    var min_tile    = origin        ;

    //一番スコアの低いものを探す
    for(int m = 0; m < 4; m++)
    {
        if (scores[m] == 0)                             continue;
        if (scores[m] > min_score)                      continue;
        if (scores[m] == min_score && cost >= min_cost) continue;

        min_score   = scores[m];
        min_cost    = cost;
        min_tile    = tiles[m];
        min_hcost   = hcosts[m];
    }

    //自身をClose
    if (origin != null)
    {
        origin.Close();
    }

    if (min_tile.GetTileType() == TileType.Walkable)
    {
        origin.SetAnalyzed(true);
        DrawLineCenterTileToTile(origin.GetCoordinate(), min_tile.GetCoordinate());
    }
    return min_tile;
}

////////////////////////////////////////////////////////////////////////
// マップを探索
////////////////////////////////////////////////////////////////////////
public static async void AnalyzeMap()
{
    LoggerForm.WriteSuccess("探索開始");

    //各タイル座標に属性を付与
    SetTileAttributesToAll();

    var startTile   = GetTileBlockByTileType(TileType.StartTile);
    var goalTile    = GetTileBlockByTileType(TileType.GoalTile );

    if(startTile == null || goalTile == null)
    {
        if(startTile == null)
        {
            LoggerForm.WriteError("Start tile not found.");
        }
        else if(goalTile == null)
        {
            LoggerForm.WriteError("Goal tile not found.");
        }
        return;
    }

    var startTileCoord  = startTile.GetCoordinate();
    var goalTileCoord   = goalTile. GetCoordinate();

    var k = 0;
    TileBlock bestTile = startTile;
    while (k < 999)
    {
        k++;
        if (bestTile == null || !IsTileBlockExists(bestTile.GetCoordinate()))
        {
            LoggerForm.WriteError("Tile not found.");
            break;
        }
        else if (bestTile.GetTileType() == TileType.GoalTile)
        {
            LoggerForm.WriteSuccess("Goal found.");
            break;
        }
        else
        {
            bestTile = GetBestTile(bestTile, k);
        }
        await Task.Delay(10);
    }
}

アルゴリズムについて

gceun-5mza3.gif
ご覧の通り、最短ルートは保証されてません。アルゴリズム(笑)です。
なぜこうなっているかというと、
7d8506b45d4dda150f4e932980f98440.png
進んでいる彼は左右上下1ブロックとスコアしか見えてません。
したがって、より良いスコアである右に進んでしまうのです。
しかしながら、全体で見れば左に進むのが明らかに最短ルートです。
こういった場合、左に進んでゴールに辿り着いた場合と
右に進んでゴールに辿り着いた場合の総タイル数を比較してルートを定める必要があります。
また、直線移動しかしません。行き止まりにも対応してません。

「最短コースは保証されていないが、最低限の分析でゴールに辿り着ける」
という側面で見れば、ダイクストラ法とAスター法のいいとこ取りだと思います。(言い訳)
また改めて時間があれば、最適化します。

といった感じで、中途半端ではありますが経路探査アルゴリズムを作ってみました。
ソースコードはGithubにあります。

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした