当たり判定に役立つかもしれない４分割セグメントツリー

本記事は、「[データ構造とアルゴリズム Advent Calendar 2020](https://qiita.com/advent-calendar/2020/str)」 3日目の記事です。
2日目の記事は @okateim 氏による「[「4人のロシア人の方法」を用いた編集距離計算の高速化](https://qiita.com/okateim/items/a1b0523c1c862009e81f)」です。
4日目の記事は @tkbtkysms 氏による「[文法圧縮の可視化ツール](https://qiita.com/tkbtkysms/items/408d6e1bc2ea3c86c2e9)」です。

# 概要

2Dゲームの当たり判定を最適化するために使った、四分木を使ったアルゴリズムについて解説します、セグメント木っぽい手法を応用しているので、オブジェクトの範囲検索に対応しています。

本記事のソースコードはVS2013、もしくは`g++ -std=c++11`での使用を想定しています。(使ったゲームの開発環境が古いせいです。すいません…)
また、簡略化のためにassert文を除去しています。

# 開発経緯

とある2Dゲームの当たり判定処理を最適化しようと思ったのですが、手頃なライブラリがなかったので自力で実装することにしました。

# 解説

オブジェクト同士の判定回数を最適化するために、画像処理などで知られる**四分木を用いた空間分割**を行います。
四分木とは、領域を再帰的に4分割する木構造のデータ構造のことです。

![David Eppstein, Public domain, via Wikimedia Commons](https://upload.wikimedia.org/wikipedia/commons/8/8b/Point_quadtree.svg "David Eppstein, Public domain, via Wikimedia Commons")
領域全体を等間隔のグリッドに分割する方式と違い、小さな領域に収まりきらない大きなオブジェクトなどを適切に扱えるという特徴があります。

四分木の構築にはヒープの構造を応用します。0を始点として、各インデックス`i`に対して、インデックス`i * 4 + 1`から`i * 4 + 4`までの４つのノードを子とし、インデックス`(i - 1) / 4`のノードを親とする、長さ $\frac{4^n-1}{4-1}$ の配列を構築します。
各要素には以下のリストを格納します。

- `origin`: そのノードに属する(領域に含まれる)オブジェクトのリスト
- `sub`: そのノードより下位のノードに属するオブジェクトのリスト[^1]

[^1]: 計算方針によっては、上位のノードに属するオブジェクトを格納する方式にした方が良い場合もあります

## オブジェクトの格納

### 賢い方法

領域の位置が固定である関係上、**ある点の座標に対応するノードは一意に求まります。**
これを利用して、挿入したいオブジェクトの左上隅と右下隅の各点が属する最下層のノード $p,q$ を求め、$p=q$ になるまでそれぞれの親を辿っていくことで、オブジェクトの属する領域を求めることができます。
必要に応じて親ノードの情報を更新し、操作を終了します。計算量は $O(\\log n)$ です。

### 普通の方法

なぜこの項目を書いたかというと先にこっちで実装してしまったからです。
根を始点とし、以下の再帰的な操作を行います。

1. 領域の中心から縦横に境界線を引いて4分割する。
2. オブジェクトが境界線を跨いでいるかどうか判定し、結果によって以下の処理を行う。
  - **オブジェクトが境界線を跨いでいる場合**[^2]：ノードの`origin`にオブジェクトを追加し、操作を終了する。
  - **それ以外の場合**：ノードの`sub`にオブジェクトを追加する。分割した4箇所のどの領域にオブジェクトが属するか判定し、該当する子ノードに対して 1. 以降の処理を繰り返す。

計算量は同じく $O(\\log n)$ です。

#### 実装

```cpp
#include <deque>
#include <cassert>
#include <functional>

// 当たり判定用4分割セグメントツリー
template<class T, class ContainerHeap = typename std::deque<T>> class CollisionDetectionTree
{
private:
    enum {
        SEGMENT_DEPTH = 4,					//! 4分木の階層数
        SIZE_HEAP = 1 << SEGMENT_DEPTH * 2,	//! ヒープのサイズ
        X_MIN = -640, X_MAX = 640,
        Y_MIN = X_MIN, Y_MAX = X_MAX
    };

    // 子ノードの最初のindexを取得
    static int getFirstChildNode(int index) { return index * 4 + 1; }

    struct {
        // origin: レイヤー内のオブジェクトのリスト、sub: 下の方にあるオブジェクトのリスト
        ContainerHeap origin, sub;
    } segmentHeap[SIZE_HEAP];

public:
    // セグメントツリーにオブジェクトを挿入
    void insert(int left, int top, int right, int bottom, const T& obj) {
        int node_current = 0, x1 = X_MIN, y1 = Y_MIN,
            x2 = X_MAX, y2 = Y_MAX;

        for (;;) {
            const int x_mid = (x2 - x1) / 2 + x1,
                      y_mid = (y2 - y1) / 2 + y1;

            // 境界線をまたいでいるか判定
            if ((getFirstChildNode(node_current) >= SIZE_HEAP) || 
                (left <= x_mid && x_mid <= right) ||
                (top <= y_mid && y_mid <= bottom)) {
                segmentHeap[node_current].origin.push_back(obj);
                break;
            }
            else {
                segmentHeap[node_current].sub.push_back(obj);
                node_current = getFirstChildNode(node_current);

                // Z型に4分割する
                if (x_mid < left) {
                    node_current += 1;
                    x1 = x_mid;
                }
                else
                    x2 = x_mid;

                if (y_mid < top) {
                    node_current += 2;
                    y1 = y_mid;
                }
                else
                    y2 = y_mid;
            }
        }
    }
```

木の高さを変更すると分割の細かさに影響します。扱うオブジェクトの数や領域の広さによって変えると良いです。このソースではデバッグ時の利便性を考えて4程度にしてあります。
木の根(インデックス`0`)を始点とし、条件に応じて再帰的に木を更新します。

木の分割に**モートン分割**という、4つの空間をZ型に配置する手法を用いています。
本記事ではあまり活用していませんが、ノードのインデックスの値から直接領域を割り出すことができるので便利です。

## 全オブジェクト同士の当たり判定

木の配列を適当に線形探索してもいいのですが、計算を省略できる場合があるので**深さ優先探索**を行います。
各ノードの要素に対し、`origin`に含まれるオブジェクト同士、`origin`に含まれる全オブジェクトから`sub`に含まれる全オブジェクトに対してそれぞれ判定することで、全オブジェクト同士の判定が可能です。

`sub`に含まれるオブジェクトが高々1つしかない場合[^3]、以降の探索を枝刈りすることで計算を省略できます。
最悪の時間計算量は $O(n^2)$ ですが、木にうまく分散した場合は最適化が見込まれます。

[^3]: 下位ノード上にあるオブジェクト同士が衝突する見込みがない場合

### 実装

```cpp
    // 全オブジェクト同士の当たり判定を実行
    template<typename F> void collideAll(F func_collide) {
        std::function<void(int)> searchHeap = [&](int node_current) {
            // ノードの衝突判定
            // もうやってるので親ノードに対しては判定しない
            for (auto it = segmentHeap[node_current].origin.cbegin();
                 it != segmentHeap[node_current].origin.cend(); ++it) {
                // 他の同階層のオブジェクトに対して
                for (auto ite = it + 1;
                     ite != segmentHeap[node_current].origin.cend(); ++ite)
                    func_collide(*it, *ite);

                // 子ノードのオブジェクトに対して
                for (const auto& j : segmentHeap[node_current].sub)
                    func_collide(*it, j);
            }

            // 下に衝突しそうなオブジェクトがある場合
            if (segmentHeap[node_current].sub.size() >= 2) {
                const int node_next = getFirstChildNode(node_current);
                // 子ノードを探索
                searchHeap(node_next);
                searchHeap(node_next + 1);
                searchHeap(node_next + 2);
                searchHeap(node_next + 3);
            }
        };

        // ヒープ全体を探索
        searchHeap(0);
    }
```

衝突判定用の関数を引数にとり、オブジェクトが衝突する可能性がある場合に呼び出しています。
再帰関数をラムダで書いていますが、もう少しいい方法があるかもしれません。

## オブジェクトの範囲検索

元がセグメントツリーなので、木に格納されているオブジェクトに対して範囲検索が可能です。
根を始点とし、以下の条件で再帰的に探索を行います。

- **ノードの示す領域が完全に範囲に含まれている場合**[^_2]：ノードの`origin`と`sub`に含まれるオブジェクトを出力し、探索を打ち切る。
- **ノードの領域が完全に範囲外の場合**：結果を無視して探索を打ち切る
- **上記以外の場合**：ノードの`origin`に含まれるオブジェクトを出力し、再帰的に子ノードの探索を行う。

**探索範囲の境界を跨ぐ(一部だけ含まれる)オブジェクトが結果に含まれることに注意してください。**
領域が完全に範囲内の場合は以下のノードの探索を省略できますが、オブジェクトを列挙する関係上、これによる最適化はなきものと考えた方がいいかもしれません。計算量は最悪の場合 $O(n)$ です。
1対全オブジェクト間の当たり判定などに使えると思います。

[^2]: 操作が葉ノード(最下位のノード)に到達した場合を含みます
[^_2]: 探索が葉ノード(最下位のノード)に到達した場合を含みます

### 実装

```cpp
    // オブジェクトの範囲検索
    template<typename F> void collideWithBB(int left, int top, int right, int bottom, F func_collide) {
        std::function<void(int,int,int,int,int)> searchHeap = [&](int node_current, int x1, int y1, int x2, int y2) {
            const int node_next = getFirstChildNode(node_current);

            if ((node_next >= SIZE_HEAP) ||
                ((left <= x1) && (top <= y1) && (right >= x2) && (bottom >= y2))) {
                // バウンディングボックスが区間を完全に含んでいる場合
                for (const auto& i : segmentHeap[node_current].origin)
                    func_collide(i);
                for (const auto& i : segmentHeap[node_current].sub)
                    func_collide(i);
            }
            else if ((left <= x2) && (top <= y2) &&
                     (right >= x1) && (bottom >= y1)) {
                // バウンディングボックスが区間の一部を含んでいる場合
                const int x_mid = (x2 - x1) / 2 + x1, y_mid = (y2 - y1) / 2 + y1;

                for (const auto& i : segmentHeap[node_current].origin)
                    func_collide(i);

                // 子ノードを探索
                searchHeap(node_next, x1, y1, x_mid, y_mid);
                searchHeap(node_next + 1, x_mid, y1, x2, y_mid);
                searchHeap(node_next + 2, x1, y_mid, x_mid, y2);
                searchHeap(node_next + 3, x_mid, y_mid, x2, y2);
            }
        };

        searchHeap(0, X_MIN, Y_MIN, X_MAX, Y_MAX);
    }
```

便宜上、矩形領域に対する全オブジェクトの当たり判定という形式をとっています。
用途に応じて、コールバック関数を呼び出すのではなく、オブジェクトを列挙した結果をリストで返す、という挙動でも良いと思います。

# 実際に使ってみた

![sshot02.jpg](https://qiita-image-store.s3.amazonaws.com/0/169123/3c6865a9-c754-92ca-0347-be70dd4783ef.jpeg)
例によって[Goluah!!](https://w.atwiki.jp/gbeginner/)

以前はCPU使用率を平均13%ほど消費していたところを、平均10%程度まで削減することに成功した。
ただ、以前はそもそも判定が必要ないオブジェクトまで総当たりで探索していたので、そちらを削減したことによる影響の方が大きいかもしれない。

# 参考文献
参考文献というより「**調べてみたらもっといいのがあった**」というのに近いですが…(座標に対応する四分木のノードを算出するための正確な方法も載っています)

- [超高速2D当たり判定プログラムを作りたかった](https://qiita.com/bearjiro/items/1078db2d78f92898b813)
- [4分木空間分割を最適化する！ - ○×つくろーどっとコム](http://marupeke296.com/COL_2D_No8_QuadTree.html)
- [四分木 - Wikipedia](https://ja.wikipedia.org/wiki/四分木)
- [*d*-ary heap - Wikipedia(英語版)](https://en.m.wikipedia.org/wiki/D-ary_heap)