障害物などを考慮した最短経路を計算する

## 前書き
壁をよけて進んでくれるようなEntityのAIをつくりたいな～っと思い、実装してみましたので共有します。

(記事を投稿するのは初めてなので暖かい目で見守っていただけるとありがたいです。)

# 環境
PocketMine-MP 4.0.0

# アルゴリズムの説明

今回用いるアルゴリズムについて簡単に説明します。

1. 初めの座標の周囲の座標を列挙する。
2. それぞれの座標とゴールとなる座標と一致しているかどうかを判定する。
3. 一致していた場合は終了し、一致していない場合は、再びその座標から周囲の座標を列挙する。

1回の移動で移動可能な座標 -> 2回の移動で移動可能な座標 -> ...
といった順番に列挙していくので、自然と最短経路を導くことができます。

＜利点＞
・障害物などを考慮した経路を計算することができる。

＜欠点＞
・計算量がそこそこ重く、快適に動作させることが難しい。

上記のアルゴリズムは、一般に幅優先探索と言われています。

## キュー(Queue)を実装する

＜キューとは?＞
要素が入ってきた順番に取り出すことができるデータ構造です。

phpの配列は優秀で、先頭の要素を取り出してくれる array_shift という関数と末尾にデータを追加する array_push という関数があるため、容易に実装が可能です。

```php:Queue.php
class Queue{

    /** @var array */
    private $array = [];

    public function isEmpty(){
        return empty($this->array);
    }

    public function push($v){
        array_push($this->array, $v);
    }

    public function pop(){
        return array_shift($this->array);
    }
}
```

## ブロック関係のAPIの実装

PMMPのAPIだけだと足りない部分があるので、それらは個別に実装します。

・固体のブロックかどうかの判定
・そのブロックへのジャンプが可能かどうかの判定
上記の判定をしてくれる関数を個別に実装しています。

```php:Blocks.php
class Blocks{

    public static function isSolid($id){
        switch($id){
            case 0:
            case 6:
            case 8:
            case 9:
            case 10:
            case 11:
                return false;
            break;

        }
        return true;
    }

    public static function goUp(int $x, int $y, int $z, World $level, int $jumpHeight = 1){
        $resultY = 0;
        for($Y = $y; $Y <= $y + $jumpHeight; $Y++){
            $airCount = 0;
            // ブロックの上にある空気ブロックの数を数える
            while($airCount < 2 and !Blocks::isSolid($level->getBlockAt($x, $Y + $airCount, $z)->getId())){
                $airCount++;
            }

            // 2マス以上の空気ブロックがあったらその座標に移動可能とする
            if($airCount >= 2){
                $resultY = $Y;
                break;
            }
        }
        return $resultY;
    }
}
```

# アルゴリズムの実装

上記のことを踏まえ、実装します。
キューに探索可能な座標を挿入し、順番に探索するようにしています。

```php
    public function getPath(Position $start, Position $goal, int $limit = 1000){
        $level = $goal->world;
        $gx = (int) $goal->x;
        $gy = (int) ($goal->y + 0.1);
        $gz = (int) $goal->z;
        while($level->getBlockAt((int) $gx, (int) $gy - 1, (int) $gz)->getId() === 0){
            $gy--;
        }

        $queue = new Queue();
        $queue->push([
            "cost" => 0,
            "x" => (int) $start->x,
            "y" => (int) $start->y,
            "z" => (int) $start->z,
        ]);

        $count = 0;
        $memo = [];
        $from = [];
        $dx = [0, 1, 0, -1];
        $dz = [1, 0, -1, 0];

        $memo[(int) $start->x][(int) $start->y][(int) $start->z] = 0;

        while(!$queue->isEmpty()){
            $top = $queue->pop();
            $count++;
            // ループ回数が上限を超えたら、探索を打ち切る
            if($limit >= 1000) break;

            if($top["y"] < 0){
                continue;
            }
            if(isset($memo[$gx][$gy][$gz])){
                break;
            }

            for($i = 0; $i < 4; $i++){
                for($j = 0; $j < 4; $j++){
                    $to = new Vector3($top["x"] + $dx[$i], $top["y"], $top["z"] + $dz[$j]);
                    $x = $to->x;
                    $y = $to->y;
                    $z = $to->z;

                    $blockId = $level->getBlockAt((int) $x, (int) $y, (int) $z)->getId();
                    if(Blocks::isSolid($blockId) === true){
                        $ry = Blocks::goUp((int) $x, (int) $y, (int) $z, $level);
                        if($ry === 0){
                            continue;
                        }else{
                            $y = (int) ($ry);

                            if(isset($memo[$x][$y][$z])){
                                continue;
                            }

                            $memo[$x][$y][$z] = $top["cost"] + 1;
                            $from[$x][$y][$z] = ["x" => $top["x"], "y" => $top["y"], "z" => $top["z"]];
                            $queue->push([
                                "cost" => $top["cost"] + 1,
                                "x" => $x,
                                "y" => $y,
                                "z" => $z,
                            ]);
                        }
                    }else{
                        if(Blocks::isSolid($level->getBlockAt((int) $x, (int)($y - 2), (int) $z)->getId()) !== true){
                            continue;
                        }elseif(Blocks::isSolid($level->getBlockAt((int) $x, (int)($y - 1), (int) $z)->getId()) !== true){
                            $y--;
                        }

                        if(isset($memo[$x][$y][$z])){
                            continue;
                        }

                        $memo[$x][$y][$z] = $top["cost"] + 1;
                        $from[$x][$y][$z] = ["x" => $top["x"], "y" => $top["y"], "z" => $top["z"]];
                        $queue->push([
                            "cost" => $top["cost"] + 1,
                            "x" => $x,
                            "y" => $y,
                            "z" => $z,
                        ]);
                    }
                }
            }
        }

        $path = [];
        $nx = $gx;
        $ny = $gy;
        $nz = $gz;
        // 経路の復元 (ゴールからスタートにさかのぼる)
        while(isset($from[$nx][$ny][$nz])){
            $pos = $from[$nx][$ny][$nz];
            $path[] = new Vector3($pos["x"] + 0.5, $pos["y"], $pos["z"] + 0.5);
            $nx = $pos["x"];
            $ny = $pos["y"];
            $nz = $pos["z"];
        }

        $reversed = array_reverse($path);
        $res = new Queue();
        foreach($reversed as $pos){
            $res->push($pos);
        }
        return clone $res;
    }
```

## 後書き

実用性はあまりありませんが、誰かの参考になれば幸いです。

Twitterに、このアルゴリズムを用いてゾンビを動かしてみた動画を載せているので、興味がある方は是非ご覧ください。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">障害物を避けてくれるゾンビを作った！(端末とサーバーが激重) <a href="https://t.co/e70kpsWMG8">pic.twitter.com/e70kpsWMG8</a></p>&mdash; ただの焼き鳥さん (@yaki_daruma02) <a href="https://twitter.com/yaki_daruma02/status/1464270089813778437?ref_src=twsrc%5Etfw">November 26, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>