[Unity] 自作物理エンジン～GJKアルゴリズム実装編～

　Unityにはもともと物理エンジンが備わっていますが、今回はそれを自作するお話です。制作に至った経緯は、PureECSという新しいUnityのプラグインにコリジョンシステムがまだ実装されておらず、なんとかしてあたり判定を得る必要が出てきたからです。今回の記事では直接的には関係がありませんが、将来的にPureECSに移植することを前提としています。

#GJKアルゴリズムで衝突判定を取る
　UnityのCollider系コンポーネントがどのようにあたり判定を取っているのかが、正直に言って全然わかりませんでした。ですので、採用するアルゴリズムには、webに既出のものから実装が比較的容易で多様性がありそうなものを選びました。（ここらへんは前回までのお話）
　そのアルゴリズムがGJKアルゴリズムと呼ばれるものです。ものすごく簡単に説明すると、『衝突を判定する2つのオブジェクトで形成される凸胞体が、原点を含むかどうかで衝突を判定する』というものです。全然わかりませんね、ちゃんと説明します。

###事前知識
　凸胞体とはなにか。これはUnityユーザーなら結構なじみがある代物です。MeshColliderコンポーネントにConvexというスイッチがありますが、あれのことです。この凸胞体は、すべてのメッシュ頂点が内包される多面体のことで、オブジェクトをラップで巻き付けたようなものを想像してください。UnityではmeshデータやメソッドからConvexHullにアクセスできる方法がないので、この凸胞体は手動で形成しなければなりません。

　ミンコフスキー差にはミンコフスキー和という兄弟がいますが、GJKアルゴリズムには差のほうしか登場しません。この和と差を説明するために、メッシュAとBを仮定します。形は何でも構いませんが、形が単純なほうがイメージしやすいので四角形と楕円で考えます。ミンコフスキー和はBの周囲にAの原点を這わせてできる軌跡です。

ミンコフスキー差は、原点で点対象させた図形A´を、同じくBの周囲を這わせた軌跡となっています。と、このミンコフスキー差は低次元で簡単な図形なら想像に容易いものですが、形が複雑になったり次元が上がったりすると、途端にその図形を描くのが難しくなります。しかし、このミンコフスキー差にはおもしろい性質があり、元の図形の頂点をうまく抽出することで簡単に合成することができます。その方法がサポート写像を利用する方法です。サポート写像の詳細は後述しますが、とりあえずミンコフスキー差は簡単に調べられるということを覚えておいてください。
　で、ミンコフスキー差なるものは分かったけどそれがどうしたの？　ええ、ごもっともなご質問でございます。このミンコフスキー差は別の特異な法則を持っています。それが、図形Bに対する図形Aのミンコフスキー差が原点を含む時、元の図形AとBは交わっている、という法則です。これがGJKアルゴリズムの肝となります。平面や立体の中に任意の点が含まれるかどうかを調べることは簡単ですし、先ほど触れたようにミンコフスキー差も簡単に求まります。

#ConvexHullな頂点情報の作成
　先にColliderに使用する頂点情報を生成します。サンプルコードは以下の通りです。抽出した頂点はScriptableObjectで保存しておきます(←値が保存できればなんでもいいです)。パフォーマンスがいらないので、ガバガバでゴリゴリな作りとなっていますがご容赦ください。抽出方法はそれほど難解ではありません。
①対象のメッシュデータからxy座標だけを抽出
②x座標を降順にソート、一番目と最後尾がx軸上で見る最遠点になる。
③上の二点を結ぶ線分から最も遠い点を配列に追加。
④このとき三点で構成される三角形の内部に含まれる点を削除する(←これらの点はどうあがいても最遠点になれない)
⑤追加した点と配列上で一つ前の点との線分で最遠点を探す。
⑥なければ次のペアで最遠点を探索、あれば③④⑤を繰り返す。
⑦xz座標軸、zy座標軸でも同じ処理
ConvexHullはほかにも抽出方法があるので、自分が理解しやすいアルゴリズムや実装が容易なものを探してみてください。

GenerateConvexMesh.cs

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

/// <summary>
/// This cariculation is for creating the list of convex vertices.
/// </summary>

    Vector2 point_start;
    Vector2 point_end;
    Vector2 point_farthest;
    Vector2 endpointsNormalized = new Vector2();
    List<Vector3> allVertices = new List<Vector3>();
    List<Vector3> vertices_axis_FRONT = new List<Vector3>();
    List<Vector3> vertices_axis_UP = new List<Vector3>();
    List<Vector3> vertices_axis_RIGHT = new List<Vector3>();

    private void GenerateConvexMesh()
    {
        convexVertices = new List<Vector3>();
        allVertices = new List<Vector3>();
        vertices_axis_FRONT = new List<Vector3>();
        vertices_axis_UP = new List<Vector3>();
        vertices_axis_RIGHT = new List<Vector3>();

        //////////// front direction
        allVertices.AddRange(bulletMesh.vertices);
        var allVerticesOrdered = allVertices.OrderBy(x => x.x);/////x軸で並び替え
        allVertices = allVerticesOrdered.ToList();

        vertices_axis_FRONT.Add(allVertices[0]);////最もx成分が小さい頂点を確保
        allVertices.RemoveAt(0);
        vertices_axis_FRONT.Add(allVertices.Last());////最もx成分が大きい頂点を確保
        allVertices.RemoveAt(allVertices.Count - 1);

        var counter = 0;
        while(counter < 1000)
        {
            for (int q = 0; q < vertices_axis_FRONT.Count - 1; q++)
            {
                point_start = new Vector2(vertices_axis_FRONT[q].x, vertices_axis_FRONT[q].y);
                point_end = new Vector2(vertices_axis_FRONT[q + 1].x, vertices_axis_FRONT[q + 1].y);
                endpointsNormalized = (point_end - point_start).normalized;
                if (DetectFarestPoint_FRONT(q, point_start))
                {
                    q--; /////最遠点が見つかれば反復処理のインデックスを1つ差し戻す
                }
            }
            counter++;
        }

        //////////// right direction///////上記のxy成分をzx成分に置き換えて同じ処理
        //////////// up direction///////上記のxy成分をzy成分に置き換えて同じ処理
        ///////コードが長すぎるのでちょっと省略していますが、頂点抽出を3方面から行っているだけの簡単なお仕事です

        convexVertices.AddRange(vertices_axis_FRONT);
        convexVertices.AddRange(vertices_axis_RIGHT);
        convexVertices.AddRange(vertices_axis_UP);/////三方面から抽出した頂点を一つのリストにする

        var hashset = new HashSet<Vector3>();
        foreach (var v in convexVertices)
        {
            hashset.Add(v);
        }
        convexVertices.Clear();///////重複している頂点を削除
        convexVertices = hashset.ToList();

    private bool DetectFarestPoint_FRONT(int index, Vector2 startPoint)
    {
        int currentFarthestPointIndex = 0;
        float currentFarthestDistance = 0f;

        for (int i = 0; i < allVertices.Count; i++)
        {
            Vector2 targetVertex2D = new Vector2(allVertices[i].x - startPoint.x, allVertices[i].y - startPoint.y);
            if (Vector2Cross(endpointsNormalized, targetVertex2D) < 0)/////探索方向を制限
            {
                var distance = Vector2.Dot(endpointsNormalized, targetVertex2D);//////正射影ベクトルで距離を測定
                if (currentFarthestDistance < distance)
                {
                    currentFarthestDistance = distance;
                    point_farthest = targetVertex2D;
                    currentFarthestPointIndex = i;//////暫定的な最遠点をキープ
                }
            }
        }

        if (currentFarthestDistance != 0)//////最遠点が見つかった場合
        {
            vertices_axis_FRONT.Insert(index + 1, allVertices[currentFarthestPointIndex]);
            allVertices.RemoveAt(currentFarthestPointIndex);
            RemoveVerticesInTriangle_FRONT(point_start, point_end, point_farthest);///////三点の内部に位置する頂点を削除
            return true;
        }
        else////////最遠点が見つからなかった場合
        {
            return false;
        }
    }

    private void RemoveVerticesInTriangle_FRONT(Vector2 p1, Vector2 p2, Vector2 p3)
    {
        for (int i = 0; i < allVertices.Count; i++)
        {
            Vector2 targetVertex2D = new Vector2(allVertices[i].x - p1.x, allVertices[i].y - p1.y);
            Vector2 vector_min_Max = p2 - p1;
            if (Vector2Cross(vector_min_Max, targetVertex2D) < 0)
            {
                targetVertex2D = new Vector2(allVertices[i].x - p2.x, allVertices[i].y - p2.y);
                Vector2 vector_Max_target = targetVertex2D - p2;
                if (Vector2Cross(vector_min_Max, targetVertex2D) < 0)
                {
                    targetVertex2D = new Vector2(allVertices[i].x - p3.x, allVertices[i].y - p3.y);
                    Vector2 vector_target_min = p1 - targetVertex2D;
                    if (Vector2Cross(vector_target_min, targetVertex2D) < 0)
                    {
                        allVertices.RemoveAt(i);
                    }
                }
            }
        }///////内外判定には外積を利用、すべての外積で正負が一致するとき任意の点は三角形の内部に存在する
    }

    public static float Vector2Cross(Vector2 lhs, Vector2 rhs)//////二次元に(勝手に)圧縮したため、Vector3.Crossは使えない。
    {
        return lhs.x * rhs.y - rhs.x * lhs.y;
    }

###抽出した結果がこちらです。

　そこそこうまく頂点を取れているのではないでしょうか。頂点配列を単純に線でつないだだけなので、ちょっと見づらいですが、元のメッシュをうまく縁取っている様子がわかります。ちなみに元のメッシュは317頂点を有していましたが、自作ConvexHullでは117頂点にまで削減されました。

#GJKアルゴリズムのサンプル
　先にサンプルコードを載せます。意外とシンプルに書けます。コード上、判定を行う自身をself、判定の相手をtargetとしています。

GJKCollisionTest.cs

    private Matrix4x4[] selfMatrix = new Matrix4x4[1];//////position, rotation, scaleはSetTRS()で設定
    private Matrix4x4[] targetMatrix = new Matrix4x4[1];

    void GJKcollisionCheck() 
    {
        var selfPosition = selfMatrix[0].GetColumn(3);//////描画にGraphics.DrawMeshInstancedを利用したので無理やりMatrix4x4の配列を使っています。
        var targetPosition = targetMatrix[0].GetColumn(3);
        Vector3 distance_SELFandTARGET = selfPosition - targetPosition;//////自分と判定対象の距離をキープしておきます。

        Quaternion quaternion_SELF = Quaternion.FromToRotation(Vector3.forward, selfMatrix[0].GetColumn(2));
        Quaternion quaternion_TARGET = Quaternion.FromToRotation(Vector3.forward, targetMatrix[0].GetColumn(2));

//////////////原点座標よりCollider頂点の座標を計算。サポート写像を探索する際の計算量を削減するためにリストを8つ用意してます(詳細後述)。
        List<Vector3>[] selfVertices = new List<Vector3>[8] { new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>()};
        Vector3 tempVertexQuaternioned;
        for (int i = 0; i < selfBulletInfo.convexVertices.Count; i++)
        {
            tempVertexQuaternioned = quaternion_SELF * selfBulletInfo.convexVertices[i];
            selfVertices[VectorDirection(tempVertexQuaternioned)].Add(tempVertexQuaternioned);
        }
        List<Vector3>[] targetVertices = new List<Vector3>[8] { new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>(), new List<Vector3>() };
        for (int i = 0; i < targetBulletInfo.convexVertices.Count; i++)
        {
            tempVertexQuaternioned = quaternion_TARGET * targetBulletInfo.convexVertices[i];
            targetVertices[VectorDirection(tempVertexQuaternioned)].Add(tempVertexQuaternioned);
        }

        Vector3 tempDirectionVector = distance_SELFandTARGET;///////最初のサポート写像探索を行う方向

        int tempDirectionClass_SELF = VectorDirection(tempDirectionVector);
        int tempDirectionClass_TARGET = VectorDirection(-tempDirectionVector);
        List<Vector3> supportPoints_SELF = new List<Vector3>();
        List<Vector3> supportPoints_TARGET = new List<Vector3>();
        List<Vector3> supportPoints = new List<Vector3>();
        int counter = 0;
        int vectorSelector = 3;
        while (counter < 30)///////サポート写像が取れなくなるまで計算しますが、30回の探索で打ち切りにしています。
        {
            float maxDot = 0;
            int tempSupportPointIndex_SELF = 0;

            for (int i = 0; i < selfVertices[tempDirectionClass_SELF].Count; i++)
            {
                var tempDot = Vector3.Dot(selfVertices[tempDirectionClass_SELF][i], tempDirectionVector);
                if (tempDot > maxDot)
                {
                    maxDot = tempDot;////////サポート写像の定義より内積が1に近い点を探しています。
                    tempSupportPointIndex_SELF = i;
                }
            }
            supportPoints_SELF.Add(selfVertices[tempDirectionClass_SELF][tempSupportPointIndex_SELF] + distance_SELFandTARGET);
///////立体Aのサポート写像を確保。Bの原点を基準にしているので、ABの原点間の距離を加算。

            maxDot = 0;
            int tempSupportPointIndex_TARGET = 0;
            for (int i = 0; i < targetVertices[tempDirectionClass_TARGET].Count; i++)
            {
                var tempDot = Vector3.Dot(targetVertices[tempDirectionClass_TARGET][i], -tempDirectionVector);
                if (tempDot > maxDot)
                {
                    maxDot = tempDot;
                    tempSupportPointIndex_TARGET = i;
                }
            }
            supportPoints_TARGET.Add(targetVertices[tempDirectionClass_TARGET][tempSupportPointIndex_TARGET]);

            supportPoints.Add(selfVertices[tempDirectionClass_SELF][tempSupportPointIndex_SELF] + distance_SELFandTARGET - targetVertices[tempDirectionClass_TARGET][tempSupportPointIndex_TARGET]);
///////立体Bのサポート写像を確保。Bの原点を基準にしているので、そのまま採用。

            if (supportPoints_SELF.Count >= 4)///////サポート写像が4つ確保できたら、原点の内外判定が行える。
            {
                if (supportPoints[vectorSelector] == supportPoints[vectorSelector - 3] ||///////もしサポート写像が直近4つのうちで重複していたら探索終了。原点も内側には存在しない。
                    supportPoints[vectorSelector] == supportPoints[vectorSelector - 2] ||
                    supportPoints[vectorSelector] == supportPoints[vectorSelector - 1])
                {
                    hitCheck = false;
                    break;
                }

                if (OriginInsideCheck(supportPoints[vectorSelector - 3], supportPoints[vectorSelector - 2], supportPoints[vectorSelector - 1], supportPoints[vectorSelector]) &&
                    OriginInsideCheck(supportPoints[vectorSelector], supportPoints[vectorSelector - 3], supportPoints[vectorSelector - 2], supportPoints[vectorSelector - 1]) &&
                    OriginInsideCheck(supportPoints[vectorSelector - 1], supportPoints[vectorSelector], supportPoints[vectorSelector - 3], supportPoints[vectorSelector - 2]) &&
                    OriginInsideCheck(supportPoints[vectorSelector - 2], supportPoints[vectorSelector - 1], supportPoints[vectorSelector], supportPoints[vectorSelector - 3]))
                {
                    hitCheck = true;
                    break;
                }

                vectorSelector++;/////////原点が外側にあるが、まだサポート写像を取れるので探索を続行。
            }

            if (supportPoints_SELF.Count == 1)///////確保したサポート写像が一つの時、サポート写像から原点に向けたベクトルが次の探索方向。
            {
                tempDirectionVector = supportPoints_TARGET[0] - supportPoints_SELF[0];
            }
            else if (supportPoints_SELF.Count == 2)////////確保したサポート写像が二つの時、二点を結ぶ直線に対して原点から下ろした垂直なベクトルの逆ベクトルが次の探索方向。
            {
                Vector3 v = supportPoints[1] - supportPoints[0];
                tempDirectionVector = (Vector3.Dot(v, -supportPoints[0]) / v.sqrMagnitude * v + supportPoints[0]) * -1; //正射影ベクトルで求まります。
            }
            else if (supportPoints_SELF.Count >= 3)///////確保したサポート写像が三つ以上なら、三点を含む面の法線ベクトルが次の探索方向。
            {
                int last = supportPoints_SELF.Count - 1;
                Vector3 n = Vector3.Cross(supportPoints[last - 2] - supportPoints[last], supportPoints[last - 1] - supportPoints[last]);
                if (Vector3.Dot(n, -supportPoints[last]) < 0)
                {
                    n *= -1;
                }
                tempDirectionVector = n;
            }
            tempDirectionClass_SELF = VectorDirection(tempDirectionVector);
            tempDirectionClass_TARGET = VectorDirection(-tempDirectionVector);

            counter++;
        }
    }

    private int VectorDirection(Vector3 vec)////////探索方向に含まれる頂点座標の仕分け。
    {
        int directionIndex = 0;
        if (vec.x >= 0)
        {
            directionIndex += 4;
        }
        if (vec.y >= 0)
        {
            directionIndex += 2;
        }
        if (vec.z >= 0)
        {
            directionIndex += 1;
        }
        return directionIndex;
    }

    private bool OriginInsideCheck(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3)
    {
        Vector3 n = Vector3.Cross(p1 - p0, p2 - p0);

        if (Vector3.Dot(-p0, n) >= 0 && Vector3.Dot(p3 - p0, n) <= 0)///////法線との内積が一致しなければ二点は面を挟んでいる。
        {
            return false;
        }
        else if (Vector3.Dot(-p0, n) <= 0 && Vector3.Dot(p3 - p0, n) >= 0)
        {
            return false;
        }
        return true;
    }

###コードの解説
　やっていることは以下の通りです。
####①立体ABの原点からColliderの頂点座標を復元
　この時、計算後の頂点座標をを八分木に分類します。こうすることで探索方向が決まった時点でどの頂点群にサポート写像が含まれているかがある程度絞り込めます。例えば探索方向のベクトルが(1, -1, -1)だとすると、八分木リスト５番目の頂点群だけを調べればいいことになります。各八分木に収容する処理が毎フレーム走ることになりますが、サポート写像を毎回すべての頂点から探索する負荷に比べれば安いものです。
　また座標を復元する際にQuaternion.FromToRotationで得られるクォータニオンを掛け合わせています。先に算出しておいたConvexHullはRotationが(0, 0, 0)の場合の頂点座標でした。立体Aが向いている方向にConvexHullの頂点たちも傾けなくてはなりません。

####②サポート写像を探索
　ミンコフスキー差の話に戻ります。次元が上がったり図形が難しくなったりすると、ミンコフスキー差は描くのが難しくなります。しかし、実は図形自体を知っている必要はどこにもありません。欲しい情報はサポート写像であって、ミンコフスキー差ではないのです。ミンコフスキー差を構成する図形のサポート写像からミンコフスキー差のサポート写像は求まります。

Support_C(\vec{v}) = Support_A(\vec{v}) - Support_B(-\vec{v})

そしてサポート写像も内積から簡単に求めることができます。

Support_X(\vec{v}) = P \in X, (p・\vec{v} ) \geq (Q・\vec{v} ), \forall Q \in X

ちょうどこんな感じでミンコフスキー差のサポート写像は合成されます。あくまでイメージですが、概念を掴むには十分でしょう。

####③探索方向を決定
　初回の探索方向は立体Bの原点から立体Aの原点に向けたベクトルを利用するといいでしょう。この方向を探索することで、合成したミンコフスキー差の中で原点から最も離れたサポート写像が得られます。
　二回目の探索方向は一回目の探索で得られたサポート写像から原点に向かうベクトルです。
　三回目の探索方向は一回目と二回目で得られたサポート写像を通る直線に原点から垂線を下ろしたときの逆ベクトルです。
　四回目以降はすべて同じです。**前回、前々回、前々々回に得られたサポート写像を含む平面の法線ベクトル(原点側に向けて)**が探索方向になります。アルゴリズムの終着点が『原点が含まれるかどうか』なので、原点方向に探索範囲が広がっていくことになります。下図にそのイメージとなります。

####④原点を含むか判定
　サポート写像が4つ得られた時点で最低でも1つの単体を形成することができます。単体(simplex)とは、立体や図形を構成する最小単位で、二次元なら三角形、三次元なら四面体が単体となります。数学には明るくないので、詳しい定義なんぞはすっ飛ばします。とにかく、新しい単体が得られた時点で原点の内外を検査し、新しい単体が得られなくなるまでこの処理を繰り返します。もちろん、原点が含まれる単体が見つかればhit判定を返し処理を終了します。新たな単体が得られなくなった時点で原点を含む単体が見つけられていなければ、立体ABはぶつかっていないことになります。
　
#とりあえずMonoBehaviourで動かしてみる
　これをいきなりECSにぶち込んでもデバックしにくいので、とりあえずMonoBehaviourで動かしてみます。

　衝突を検知すると光ります。無数の線がオブジェクトを取り巻いていますが、これは確保されたサポート写像を順番に繋いだものです。ちょっと感度が良くない角度がありますが、衝突判定を返してくれています。
　このサンプルでは30回を目途に反復処理を打ち切っています。言い換えると、30回サポート写像を確保しても原点を含むかわからない場合、衝突していないものとする、ということです。なぜ30回かというと、これ以下だと目に見えて感度が悪くなったためです。
　それでは、30回を最高動作回数として処理時間を見てみます。だいたい0.10msで推移していました。時折0.15msぐらいかかることはありましたが、0.10ms前後で動作するようです。Unityオリジナルの衝突判定がどれくらいの処理速度かわからないので、比較対象に困ります。体感的には思ったほど重くないと思いました。

#ECSおよびJobに移植する(まだやってないです)
　思ったほど重くない、なんて書きましたがMonoBehaviourなら重くないという意味です。ECSでは....ちょっと厳しいかもしれません。判定1回に0.10msですから100回判定を行うと単純に10msかかります。そもそもECSは大量のオブジェクトをメモリアロケーションを工夫して動かす代物です。ECSに導入するには衝突判定を行う対象を100組までに絞り込む別のアルゴリズムが必要となるでしょう。
　この上限100回は自分にはECSには少なすぎるように思いますが、みなさんはどう思いますか？　100回と仮定しましたが、当然他の処理も走るわけですから、この上限はもっと下になります。この上限をもう少し上げるには、以下のような方法が取れるんじゃないかと思っています（まだ試していません）。
　①アルゴリズムを改良する
　②Jobで並列処理・バックグラウンド処理
　③GPUで回す
できれば③に持ち込みたく思っていますが、スクリプトを見ればわかるように、if分岐を多用しています。これだけのif分岐をGPUが耐えてくれるかわかりません。
　とにかく、ECSであたり判定を取ることは理論上可能です。が、まともには動かないでしょう。自分のプロジェクトでは最大10000Entityの同時処理を目指しているので、このシステムを導入するにはもう少し改良が必要そうです。何かいいアイデアがありましたら、コメントもらえると幸いです。では。