領域四分木で当たり判定を高速化する 実装編

# はじめに
この記事は [理論編][prev] の続きです。理論編の内容を完全に理解する必要はありませんが、必要に合わせて一緒に読むとわかりやすいと思います。
コードだけ見たい人は最初の数節を飛ばしてください。

# パフォーマンスの予想
N個のオブジェクトがあるとします。普通に当たり判定を計算すると $N^2$ 回の判定が必要なのは明らかです。空間を 8x8 に分割する場合、オブジェクトの大きさに対して空間の単位長方形の大きさをいい感じに選ぶことで、最小空間当たり大体 $N/64$ 個のオブジェクトが入っていると考えられます。(もちろん、空間を跨ぐオブジェクトはいくつか存在しますが、ほとんどのオブジェクトは葉空間に配置されるとします) すると、その小空間同士で当たり判定を計算すればいいので判定回数は、$ 64 \times (N / 64)^2 = N^2 / 64 $ と、愚直にやるより 1/分割数 だけ少ない回数になることがわかります。[理論編][prev]で述べたように、N個のオブジェクトを空間に配置するのは $O(N)$ できるので事前計算にかかる時間は無視できます。
分割数を増やせば増やすだけ早くなるというわけではなく、オブジェクトの大きさに合わせて空間のサイズを選ぶことが重要になります。

# 実装方針について
### オブジェクトの管理方法と、毎フレームごとの更新方法
選択肢1は、オブジェクトをweak_ptrなどスマートポインタで持っておき、オブジェクトの最初の出現フレームで木に登録します。以降は毎フレームオブジェクトの位置やポインタの有効性を確認し、木から削除するか移動に合わせて対応する木のノードからノードへオブジェクトを移し更新します。
この実装の場合、木の1ノードが持つオブジェクトのリストには、途中要素の削除が高速に行われる必要があるためリンクリストでの実装を余儀なくされます。途中要素の定数時間削除さえできれば、更新操作は登録操作とほぼ変わらないので $O(N)$ でできます。動かないオブジェクトが多い場合、動いたオブジェクトだけわかるのならそこのみを更新すればいいので更新が速い利点はあります。一方リンクリストを使うせいで $O(N^2)$ 回ある当たり判定の計算のループのイテレーションが定数倍で激烈に遅いためパフォーマンスが後述する方法と比べてかなり悪いです。

選択肢2は、木を毎回構築することです。選択肢1は更新操作が最悪 $O(N)$ なのですが、結局毎フレームの当たり判定の構築も $O(N)$ かつ、更新の場合は途中要素の削除→新しい場所への追加 と追加だけする構築よりも大変です。よって実は **毎フレーム木を再構築したほうが速いし楽** であることに気づきます。すると、木が保持するオブジェクトの生存期間については考えなくてよい(当たり判定を計算する直前に、有効なオブジェクトだけで木を構築すればいい) ので、生ポインタで持つことができます。さらに、木の1ノードが持つオブジェクトのリストは末尾の追加だけ早ければいいのでvectorを使うことができ、イテレーションが高速になります。

よく紹介されている方法は選択肢1の実装なのですが、今回は上記の考察より選択肢2を実装してみます。広まれ、この実装…！

:::note warn
選択肢1はもう少しいい実装があるかもしれません
更新(途中要素の削除や追加)は全体で $o(N^2)$ でできればいいのでリンクリストじゃなくていいかもしれないです。また、更新した部分だけを当たり判定で計算すれば、全く動かないオブジェクトが多い場合は効率的かもしれません。ノード間移動をしなかったとしても、少しでも動けば当たり判定の再計算が必要なので用途は限定的だと思いますが...。
:::
  
# 実装
空間の最大分割数は $2^{16} \times 2^{16}$ まで許容できるように書いてますが、分割数を固定して使う場合は処理の一部や配列の一部を省けます。大体の場合 $2^{4} \times 2^{4}$ ぐらいだと思うので…

今回四分木で管理するオブジェクトはこちらにします。まあ例なので適当です。

```mycircle.hpp
#pragma once

// 当たり判定のプリミティブ
struct MyCircle
{
	float x, y, r;
};

// 当たり判定を持つオブジェクト
class MyObject
{
private:
	float x, y, r;
public:
	// 当たり判定を返す
	MyCircle GetCircle() const
	{
		return MyCircle{ x, y, r };
	}
	int hit_count = 0;
	MyObject(float x, float y, float r) : x{ x }, y{ y }, r{ r } {}
};

inline bool HitTestCircle(const float x1, const float y1, const float r1, const float x2, const float y2, const float r2)
{
	const float dx = x2 - x1, dy = y2 - y1;
	const float r = r1 + r2;
	return dx * dx + dy * dy <= r * r;
}
```

今回の例では 1280x720の画面で、オブジェクトはこの中を自由に動き回ると仮定します。
```window_def.hpp
#pragma once
inline const int WIN_W = 1280;
inline const int WIN_H = 720;
```

さて、以下が四分木の実装例です。ビット演算がすごいですが調べたらでてきます。
```qtree.hpp
#pragma once

#include "window_def.h"
#include "mycircle.h"

// 最大深さdepthの四分木のセル数, 1 + 4 + 16 + ... + 4^depth = {(2^2)^(depth+1)) - 1} / 3
static constexpr uint64_t sum_of_tree(const uint64_t depth) {
    return ((((uint64_t)1) << (2 * depth + 2)) - 1) / 3;
}

// calc 2^depth, 最大深さdepthの四分木の一辺の分割数に相当
static constexpr uint64_t MAX_SPLIT(const uint64_t depth) {
    return ((uint64_t)1) << depth;
}

static const uint64_t offset[16] = { 0, 1, 5, 21, 85, 341, 1365, 5461, 21845, 87381, 349525, 1398101, 5592405, 22369621, 89478485, 357913941 }; // (4^n - 1) / 3

// for GetMSBPos(), magic-number table
static const unsigned char s_abyLog2[32] = {
     1,  2, 29,  3, 30, 20, 25,  4, 31, 23, 21, 11, 26, 13, 16,  5,
    32, 28, 19, 24, 22,  10, 12, 15, 27, 18,  9, 14, 17,  8,  7,  6
};

/*
 * T型のオブジェクトを格納する、最大深さMAX_Dの領域四分木
 * 最大 2^16 x 2^16 までの分割(= MAX_D<=15)に対応
 * 例えば 8x8分割なら MAX_D=3
 */
template <typename T, unsigned int MAX_D>
class QTree
{
public:
    /*
     * オブジェクトを四分木に登録する
     */
    void Push(T* obj)
    {
        const auto c = obj->GetCircle();
        const auto id = GetCellID(c.x - c.r, c.y - c.r, c.x + c.r, c.y + c.r);
        this->cell[id].push_back(obj);
    }

    /*
     * 次フレームに備えて木の状態をリセットする
     */
    void Cleanup()
    {
        for (int i = 0; i < sum_of_tree(MAX_D); i++) {
            //cell[i].resize(0);
            cell[i].clear();
        }
    }

private:
    /*
     * 深さdepthでz値がz_valueを持つノードに登録されたオブジェクトに対して衝突判定を行う
     */
    void HitTest(uint64_t depth, uint64_t z_value, std::vector<T*>& stack)
    {
        const uint64_t id = GetCellID(depth, z_value);
        const std::vector<T*>& lst = cell[id];
        const uint64_t lst_size = lst.size();

        // 衝突判定をしていく
        for (int i = 0; i < lst_size; i++) {
            T* const p1 = lst[i];
            const auto c1 = p1->GetCircle();

            // スタックとノード
            for (int j = 0; j < stack.size(); j++) {
                const auto c2 = stack[j]->GetCircle();
                if (HitTestCircle(c1.x, c1.y, c1.r, c2.x, c2.y, c2.r)) {
                    p1->hit_count++;
                    stack[j]->hit_count++;
                }
            }
            // ノード内同士
            for (int j = i + 1; j < lst_size; j++) {
                const auto c2 = lst[j]->GetCircle();
                if (HitTestCircle(c1.x, c1.y, c1.r, c2.x, c2.y, c2.r)) {
                    p1->hit_count++;
                    lst[j]->hit_count++;
                }
            }
        }

        // 深さが最大なら終了
        if (depth == MAX_D)
            return;

        // スタックに積む
        for (int i = 0; i < lst.size(); i++) {
            stack.push_back(lst[i]);
        }

        // 子空間へ再帰
        for (uint8 i = 0; i < 4; i++) {
            this->HitTest(depth + 1, (z_value << 2) + i, stack);
        }
        // スタックから降ろす
        for (int i = 0; i < lst_size; i++) {
            stack.pop_back();
        }
        return;
    }

public:
    /*
     * 登録されている全オブジェクトに対して衝突判定を行う
     */
    void HitTest()
    {
        std::vector<T*> stack;
        this->HitTest(0, 0, stack);
    }

private:
    std::vector<T*> cell[sum_of_tree(MAX_D)]; // tree of vector<T*>

    // 深さとz値からノード番号を得る
    static constexpr uint64 GetCellID(uint64_t depth, uint64_t z_value)
    {
        return  offset[depth] + z_value;
    }

    // 最大16bitの入力を、bitを一つ飛ばしにして出力する。
    static uint64_t BitSeparate16(uint64_t n)
    {
        n = (n | (n << 8)) & (uint64_t)0x00FF00FF; // 9~16bit
        n = (n | (n << 4)) & (uint64_t)0x0F0F0F0F; // 5~8bit
        n = (n | (n << 2)) & (uint64_t)0x33333333; // 3~4 bit
        n = (n | (n << 1)) & (uint64_t)0x55555555; // 1~2bit
        return n;
    }

    // 最大16bitの入力x, yから32bitのz値を得る
    // x, yは最小分割単位のグリッド上で左上から右にx, 下にy移動の意味
    static constexpr uint64_t GetZ(uint64_t x, uint64_t y)
    {
        return (BitSeparate16(x) | (BitSeparate16(y) << 1));
    }

    // 32bitの整数のMSBの位置を1-indexedで得る
    // 0x00000001の場合は1を返す
    // 0x00800F00の場合は24を返す
    // 0x00000000の場合は見定義！
    static constexpr uint64_t GetMSBPos(uint32_t uVal)
    {
        /* Propagates MSB to all the lower bits. */
        uVal |= (uVal >> 1); // 2~1 bit
        uVal |= (uVal >> 2); // 4~3 bit
        uVal |= (uVal >> 4); // 8~5 bit
        uVal |= (uVal >> 8); // 16~9 bit
        uVal |= (uVal >> 16); // 32~17 bit
        /* Turns off all the bits except MSB. */
        uVal >>= 1;
        uVal++;
        /* Parameter uVal must be a power of 2. */
        const uint32_t MAGIC = 0x07D6E531;
        uVal = (MAGIC * uVal) >> 27;

        return (int64_t)s_abyLog2[uVal & 0x1F];
    }

    // 対角の2点からノード番号を得る
    // (x1, y1) <= (x2, y2)とする
    static uint64 GetCellID(float x1, float y1, float x2, float y2)
    {
        // 単位長方形の幅と高さ
        constexpr float DX = static_cast<float>(WIN_W) / MAX_SPLIT(MAX_D);
        constexpr float DY = static_cast<float>(WIN_H) / MAX_SPLIT(MAX_D);
        constexpr int64_t MAX_SPLIT_NUM = MAX_SPLIT(MAX_D);
        // 0 ~ MAX_SPLIT-1 のグリッド座標へ変換
        int64_t ix1 = (int64_t)(x1 / DX);
        int64_t iy1 = (int64_t)(y1 / DY);
        int64_t ix2 = (int64_t)(x2 / DX);
        int64_t iy2 = (int64_t)(y2 / DY);
        if (ix1 < 0 || ix2 >= MAX_SPLIT_NUM) {
            ix1 = ix1 < 0 ? 0 : ix1 >= MAX_SPLIT_NUM ? MAX_SPLIT_NUM - 1 : ix1;
            ix2 = ix2 < 0 ? 0 : ix2 >= MAX_SPLIT_NUM ? MAX_SPLIT_NUM - 1 : ix2;
        }
        if (iy1 < 0 || iy2 >= MAX_SPLIT_NUM) {
            iy1 = iy1 < 0 ? 0 : iy1 >= MAX_SPLIT_NUM ? MAX_SPLIT_NUM - 1 : iy1;
            iy2 = iy2 < 0 ? 0 : iy2 >= MAX_SPLIT_NUM ? MAX_SPLIT_NUM - 1 : iy2;
        }
        // 0 <= ix1, iy1, ix2, iy2 < MAX_SPLIT_NUM <= 2^16
        const uint64_t x = ix1 xor ix2;
        const uint64_t y = iy1 xor iy2;
        const uint64_t xy = x | y;
        const uint64_t z = GetZ(ix1, iy1);
        if (xy == 0) {
            return offset[MAX_D] + z;
        }
        uint64_t msb_pos = GetMSBPos(static_cast<uint32_t>(xy)); // 1-indexed, assume xy < 2^32
        uint64_t L = MAX_D - msb_pos;
        return offset[L] + (z >> (msb_pos << 1));
    }
};
```
MSBやz値の計算は条件分岐を一切使わないビット演算によって高速に処理されます。軸並行矩形→グリッド座標への変換はfloatの除算と条件分岐がありますが、これは仕方ないやつです。

# 実験
いくつかの当たり判定の実装を比べてみます。オブジェクトを大量に生成して、1フレーム当たり何マイクロ秒で処理できるかを測っていきます。問題設定として、オブジェクトの大きさは乱数で多少変えるがほぼ小さく、空間に均一に分布しているとします。
60fpsの場合1フレームは16.6ミリ秒=16666マイクロ秒です。描画にかかる時間などもあるのでこの時間全てを当たり判定に使えるわけではありません。この時間を超えるとゲームでは使えないということになります。
OpenSiv3Dを用いて可視化しましたが、データの生成/描画に関する時間が影響しないように計測しています。
以下、比較する実装
1. 単純な総当たり計算
1. 紹介した上記のコード

C++20, MSVCv143にてReleaseビルドして実行しました。コードは[GitHub](https://github.com/naoppy/QTreePerformanceTest)にあげてます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/212534/4d4a51e1-da51-7499-1d58-28f0c5241848.png)

このような感じで大量のオブジェクトがあり、当たっていないものは緑、当たっている場合は赤で描画しています。

# 結果
単位は当たり判定にかかった1回の時間 (マイクロミリ秒) です。
| N | 単純な総当たり | 紹介した実装 | 倍率
| ---: | ---: | ---: | :---: |
| 100 | 5 | 5 | 1.00 |
| 1000 | 360 | 70 | 5.14 |
| 5000 | 9000 | 1200 | 7.50 |
| 10000 | 36000 | 5300 | 6.79 |
| 15000 | 80000 | 13500 | 5.93 |

計算量は変わりないのにも関わらず、定数倍はかなり違いますね！しかも、$N$ が小さい領域でも総当たり以上のパフォーマンスがでていることがわかります。ただ、かかった時間を見ると $N$ が $1000$ 以下なら単純な総当たりでも充分であることもわかると思います。

結果には載せていないのですが、vectorの代わりに自作のコンテナを使ったり色々高速化を試しましたが、結論としては線形アクセスならvector使っとけばOKという感じです。

<!-- 
### 蛇足：分割数が少ない場合、bit演算の結果を事前計算して配列を見る
グリッド座標x,yを1bitずつずらすことで最も深い場所のz値を計算しました。これはbit演算で実現できるので充分速いのですが、8x8程度の分割数ですとこれを事前計算して配列でもっておくほうが速いと思われます。特にz値は8x8なら0~63の値をとるので、1byte配列で持てば充分です。

また、(x1 xor x2)||(y1 xor y2) のMSBを見るのもちょっと面倒なので、これも事前計算して配列で持ちましょう。x,yは8分割されると0~7になるので、3bitです。よってbitごとの演算の結果も3bitになるので、2^3 要素の配列でもっておけばMSBを見るのは爆速になりますね。

...と、ここまですると最早z値である意味がなくなってしまいました。z値はx,yから高速に計算できることと親の階層情報を引き継ぐことが重要な要素でしたが、前者は事前計算した配列を使えばいいので後者の特性のみを持つ値でもいいかもしれません。まあこの節で説明したことは前述した通り毎フレーム $O(N)$ 回しか呼ばれないのでやっても全然速くならないです。本当にやらなくていい。-->

# その他の派生形
グループAとグループBの間での当たり判定をしたいときは各ノードがAのリストとBのリストを持って、スタックも2つ使ってやれば効率的。

非常に小数のグループA 対 多数のグループB の当たり判定の場合、Bだけ木に入れたあとAを1つずつ取り出して、Aが入るノードの子孫全てと祖先全てのノードのBに対してそれぞれ辿って判定すればよい。

# バグ報告、待ってます…😿
読んでいただきありがとうございました。

[prev]: https://qiita.com/naoppy/items/a2a25a51c73dad4079d9

# 参考文献
- MSBの計算

https://jp.quora.com/%E6%9C%80%E4%B8%8A%E4%BD%8D%E3%83%93%E3%83%83%E3%83%88-MSB-%E3%81%AE%E4%BD%8D%E7%BD%AE%E3%82%92%E6%A4%9C%E5%87%BA%E3%81%99%E3%82%8B%E5%8A%B9%E6%9E%9C%E7%9A%84%E3%81%AA%E6%96%B9%E6%B3%95%E3%81%AF%E3%81%82%E3%82%8A

- BitSeparate32の実装

http://marupeke296.com/COL_2D_No8_QuadTree.html