👌

Unityを描画装置として用いて物理エンジンを自作してみた(Part2)

に公開
2026/01/05

はじめに

こんにちは、enari_Kと申します。普段は東京科学大学(旧東工大)で材料工学を専攻する傍らtraPというサークルでゲーム制作、競プロ、kaggle、web開発などに取り組んでいます。今回は、現在製作中の自作物理エンジンについて話していこうと思います。

この記事は連載記事です。
Part1を読んでいない方はこちらから。

球体同士の衝突

さて、物体の衝突です。前回のように箱と箱の衝突を扱ってもいいのですが、箱同士の衝突の判定から物体の衝突の影響まで調べるのはちょっと情報量が多すぎます。ですので、いったん球体と球体の衝突として扱います。

衝突する物体をAとBとし、衝突後の成分には'をつけます。成分を以下のように定義します。
\boldsymbol{v_A}:衝突前のAの速度ベクトル
\boldsymbol{v_B}:衝突前のBの速度ベクトル
\boldsymbol{v_A'}:衝突後のAの速度ベクトル
\boldsymbol{v_B'}:衝突後のBの速度ベクトル
\boldsymbol{v_{rel}}:衝突前のAに対するBの相対速度
\boldsymbol{v_{rel}'}:衝突後のAに対するBの相対速度
\boldsymbol{\omega_A}:衝突前のAの角速度
\boldsymbol{\omega_B}:衝突前のBの角速度
\boldsymbol{\omega_A'}:衝突後のAの角速度
\boldsymbol{\omega_B'}:衝突後のBの角速度
\boldsymbol{I_A}:Aの慣性モーメント
\boldsymbol{I_B}:Bの慣性モーメント
\boldsymbol{J} = j\boldsymbol{n}: 物体Bに与えられる撃力ベクトル(力積)。
(物体Aには、作用反作用の法則で -\boldsymbol{J} = -j\boldsymbol{n} が働きます。)(※\boldsymbol{n}は単位ベクトル)

さて、この物理エンジン上でどのように物体同士の衝突を処理するかということですが、撃力を求めることによって、撃力を衝突に関わった2物体について加えることで処理することとします。

F = m \cdot a

上記の式が前回出てきたと思います。これを展開すると、

\boldsymbol{F} = m \frac{\Delta \boldsymbol{v}}{\Delta t}
\boldsymbol{F} \Delta t = m \Delta \boldsymbol{v}

上記式が導出されます。ここで、力積の定義については以下です。

\boldsymbol{J} = \boldsymbol{F} \Delta t

より

\boldsymbol{J} = m \Delta \boldsymbol{v}

よって

\Delta \boldsymbol{v} = \frac{\boldsymbol{J}}{m}

さて、ここで今回力ではなく力積を扱う理由について軽く説明すると定義のように、\boldsymbol{F}に微小量である\Delta tがかけられることによって、力積は小さくなるので\boldsymbol{F}が大きくなった場合でも扱いやすくなり、オーバーフローや計算量の増大などを防げます。また、衝突時の瞬間的な大きな力を直接扱う代わりに、運動量の変化として捉えることで計算を簡略化できるからです。

さて、並進速度の変化は(運動量保存則より)
物体A: \boldsymbol{v}_A' = \boldsymbol{v}_A - \frac{j\boldsymbol{n}}{m_A}
物体B: \boldsymbol{v}_B' = \boldsymbol{v}_B + \frac{j\boldsymbol{n}}{m_B}
角速度の変化には、慣性テンソルの逆行列 \boldsymbol{I}^{-1} とトルク((\boldsymbol{N} \Delta t =)\boldsymbol{r} \times \boldsymbol{J})を用います。
物体A: \boldsymbol{\omega}_A' = \boldsymbol{\omega}_A - \boldsymbol{I}_A^{-1} (\boldsymbol{r}_A \times j\boldsymbol{n})
物体B: \boldsymbol{\omega}_B' = \boldsymbol{\omega}_B + \boldsymbol{I}_B^{-1} (\boldsymbol{r}_B \times j\boldsymbol{n})

\boldsymbol{v}_{rel}' = (\boldsymbol{v}_B' + \boldsymbol{\omega}_B' \times \boldsymbol{r}_B) - (\boldsymbol{v}_A' + \boldsymbol{\omega}_A' \times \boldsymbol{r}_A)
\begin{aligned} \boldsymbol{v}_{rel}' = \biggl( (\boldsymbol{v}_B + \frac{j\boldsymbol{n}}{m_B}) + (\boldsymbol{\omega}_B + j\boldsymbol{I}_B^{-1}(\boldsymbol{r}_B \times \boldsymbol{n})) \times \boldsymbol{r}_B \biggr) \\ - \biggl( (\boldsymbol{v}_A - \frac{j\boldsymbol{n}}{m_A}) + (\boldsymbol{\omega}_A - j\boldsymbol{I}_A^{-1}(\boldsymbol{r}_A \times \boldsymbol{n})) \times \boldsymbol{r}_A \biggr) \end{aligned}
\boldsymbol{v}_{rel}' = \underbrace{(\boldsymbol{v}_B + \boldsymbol{\omega}_B \times \boldsymbol{r}_B) - (\boldsymbol{v}_A + \boldsymbol{\omega}_A \times \boldsymbol{r}_A)}_{\text{衝突前の} \boldsymbol{v}_{rel}} + j \underbrace{\left[ \frac{\boldsymbol{n}}{m_A} + \frac{\boldsymbol{n}}{m_B} + (\boldsymbol{I}_A^{-1}(\boldsymbol{r}_A \times \boldsymbol{n})) \times \boldsymbol{r}_A + (\boldsymbol{I}_B^{-1}(\boldsymbol{r}_B \times \boldsymbol{n})) \times \boldsymbol{r}_B \right]}_{\text{係数ベクトル } \boldsymbol{K}}
\boldsymbol{v}_{rel}' = \boldsymbol{v}_{rel} + j \boldsymbol{K}

ここで、「跳ね返り」のルール(制約)を適用します。
「衝突後の法線方向の速度は、衝突前の -e 倍になる」 というルールです。(前回、地面に対するyの速度(=法線速度)を-e倍しましたよね。それと同じです。)

\boldsymbol{v}_{rel}' \cdot \boldsymbol{n} = -e (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})

式 \boldsymbol{v}_{rel}' = \boldsymbol{v}_{rel} + j \boldsymbol{K} を代入して内積をとります。

(\boldsymbol{v}_{rel} + j \boldsymbol{K}) \cdot \boldsymbol{n} = -e (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})
\boldsymbol{v}_{rel} \cdot \boldsymbol{n} + j (\boldsymbol{K} \cdot \boldsymbol{n}) = -e (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})
j (\boldsymbol{K} \cdot \boldsymbol{n}) = -e (\boldsymbol{v}_{rel} \cdot \boldsymbol{n}) - (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})
j (\boldsymbol{K} \cdot \boldsymbol{n}) = -(1 + e) (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})
j = \frac{-(1 + e) (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})}{\boldsymbol{K} \cdot \boldsymbol{n}}
\boldsymbol{K} \cdot \boldsymbol{n} = \left( \frac{\boldsymbol{n}}{m_A} + \frac{\boldsymbol{n}}{m_B} \right) \cdot \boldsymbol{n} + \left( (\boldsymbol{I}_A^{-1}(\boldsymbol{r}_A \times \boldsymbol{n})) \times \boldsymbol{r}_A \right) \cdot \boldsymbol{n} + \dots

\boldsymbol{n} \cdot \boldsymbol{n} = 1 (\boldsymbol{n}は単位ベクトルなため大きさは1)なので、質量の項は \frac{1}{m_A} + \frac{1}{m_B} になります。

j = \frac{-(1 + e) (\boldsymbol{v}_{rel} \cdot \boldsymbol{n})}{ \frac{1}{m_A} + \frac{1}{m_B} + \left( (\boldsymbol{I}_A^{-1}(\boldsymbol{r}_A \times \boldsymbol{n}) \times \boldsymbol{r}_A) + (\boldsymbol{I}_B^{-1}(\boldsymbol{r}_B \times \boldsymbol{n}) \times \boldsymbol{r}_B) \right) \cdot \boldsymbol{n} }

これで、かけるべき撃力は求まりましたね!よって、球体同士ならば衝突点は簡単に求まるので必要な物理量は揃いました。

球体同士の衝突を扱うコード

管理クラス

using UnityEngine;

[System.Serializable]
public class MyRigidBody
{
    // --- 状態変数 (State Variables) ---
    public Vector3 Position;           // 位置 x
    public Quaternion Orientation;     // 姿勢 q (回転)
    public Vector3 LinearVelocity;     // 速度 v
    public Vector3 AngularVelocity;    // 角速度 ω

    // --- 定数・物理プロパティ (Constants) ---
    public float Mass = 1.0f;          // 質量 m
    
    public float Restitution = 0.2f;
    // 慣性テンソル (Local座標系での定数)
    // 直方体の場合、幅・高さ・奥行きから計算できる対角行列
    public Matrix4x4 InverseInertiaTensorLocal; 

// 【追加】半径
    public float Radius = 0.5f;      // 球としての半径

    // --- ワーク変数 (計算用) ---
    private Vector3 _forceAccumulator;  // 蓄積された力 F
    private Vector3 _torqueAccumulator; // 蓄積されたトルク τ

// 【変更】コンストラクタを球用に調整
    public MyRigidBody(Vector3 position, float radius, float mass)
    {
        Position = position;
        Radius = radius;
        Mass = mass;
        
        // 球の慣性モーメント: I = (2/5) * m * r^2
        float I = (2.0f / 5.0f) * mass * radius * radius;
        
        InverseInertiaTensorLocal = Matrix4x4.identity;
        InverseInertiaTensorLocal[0, 0] = 1.0f / I;
        InverseInertiaTensorLocal[1, 1] = 1.0f / I;
        InverseInertiaTensorLocal[2, 2] = 1.0f / I;
        InverseInertiaTensorLocal[3, 3] = 1.0f;
    }
    // 力を加えるメソッド (重心以外の点に加えると回転が生じる)
    public void AddForceAtPoint(Vector3 force, Vector3 point)
    {
        _forceAccumulator += force;

        // 重心(Position)から力点(point)へのベクトル r
        Vector3 r = point - Position;

        // トルク τ = r × F (外積)
        _torqueAccumulator += Vector3.Cross(r, force);
    }

    // 積分ステップ (物理世界の時間を進める)
    public void Integrate(float dt)
    {
        // 1. 並進運動の更新 (F = ma)
        // 加速度 a = F / m
        Vector3 acceleration = _forceAccumulator / Mass;
        LinearVelocity += acceleration * dt;
        Position += LinearVelocity * dt;

        // 2. 回転運動の更新 (τ = I * dω/dt)
        // ここが難しいポイント:慣性テンソルは「回っている物体」と一緒に回るため、
        // World座標系での慣性テンソル逆行列を毎フレーム計算し直す必要がある。
        // I_world^-1 = R * I_local^-1 * R^T (Rは回転行列)
        
        Matrix4x4 R = Matrix4x4.Rotate(Orientation); // クォータニオンから回転行列へ
        Matrix4x4 InverseInertiaTensorWorld = R * InverseInertiaTensorLocal * R.transpose;

        // 角加速度 α = I^-1 * τ
        // Matrix4x4.MultiplyVector で行列とベクトルの掛け算
        Vector3 angularAcceleration = InverseInertiaTensorWorld.MultiplyVector(_torqueAccumulator);
        
        AngularVelocity += angularAcceleration * dt;

        // ドラッグ(空気抵抗的な減衰)を入れないと無限に回転し続けるので少し入れる
        AngularVelocity *= 0.99f; 

        // 3. 姿勢(クォータニオン)の更新
        // dq/dt = 0.5 * ω * q
        // 小さい回転を加算する処理
        Quaternion deltaQ = new Quaternion(
            AngularVelocity.x * dt * 0.5f,
            AngularVelocity.y * dt * 0.5f,
            AngularVelocity.z * dt * 0.5f,
            0 // w成分は近似的に0で扱う(微小回転のため)
        );
        
        // クォータニオンの掛け算で回転を合成
        // 注意: 単純な足し算ではなく、今の姿勢に微小回転を掛ける
        // ここでは実装簡易化のため、Unityの内部計算に近い形「q_new = q + (0.5 * w * q * dt)」を簡略化した以下を採用
        
        // ωをクォータニオン化して合成する一般的な手法
        // q_new = q_old + (dq/dt * dt)
        Quaternion qDot = deltaQ * Orientation; 
        
        Orientation.x += qDot.x;
        Orientation.y += qDot.y;
        Orientation.z += qDot.z;
        Orientation.w += qDot.w;

        // 正規化(大きさを1に保たないと計算誤差で形が歪む)
        Orientation.Normalize();

        // 4. 力をリセット
        _forceAccumulator = Vector3.zero;
        _torqueAccumulator = Vector3.zero;
    }

    // ワールド空間での逆慣性テンソルを取得するヘルパー
    public Matrix4x4 GetInverseInertiaTensorWorld()
    {
        Matrix4x4 R = Matrix4x4.Rotate(Orientation);
        return R * InverseInertiaTensorLocal * R.transpose;
    }
}

衝突用クラス

using UnityEngine;

public static class CollisionResolver
{
    // 2つの球体の衝突を解決する
    public static void ResolveCollision(MyRigidBody bodyA, MyRigidBody bodyB)
    {
        // 1. 衝突判定 (Detection)
        // 中心間の距離 < 半径の和 なら衝突
        Vector3 midLine = bodyA.Position - bodyB.Position;
        float dist = midLine.magnitude;
        float totalRadius = bodyA.Radius + bodyB.Radius;

        if (dist >= totalRadius) return; // 衝突していない

        // 2. 衝突情報の準備
        Vector3 normal = midLine.normalized; // BからAへの法線
        // 衝突点 (Contact Point): ボールの表面同士が触れている点
        // 簡単のため、Bの中心からAに向かって半径分進んだ場所とする(近似)
        Vector3 contactPoint = bodyB.Position + (midLine.normalized * bodyB.Radius);

        // 3. インパルス計算 (Resolution)
        
        // 重心から衝突点へのベクトル r
        Vector3 rA = contactPoint - bodyA.Position;
        Vector3 rB = contactPoint - bodyB.Position;

        // 相対速度 v_rel
        // v_point = v_cm + ω x r
        Vector3 velA = bodyA.LinearVelocity + Vector3.Cross(bodyA.AngularVelocity, rA);
        Vector3 velB = bodyB.LinearVelocity + Vector3.Cross(bodyB.AngularVelocity, rB);
        Vector3 relativeVelocity = velA - velB;

        // 近づいている場合のみ計算(離れようとしているなら何もしない)
        float velocityAlongNormal = Vector3.Dot(relativeVelocity, normal);
        if (velocityAlongNormal > 0) return;

        // 反発係数 (低い方を採用するのが一般的)
        float e = Mathf.Min(bodyA.Restitution, bodyB.Restitution);

        // インパルススカラー j の分子
        float j = -(1 + e) * velocityAlongNormal;

        // 分母 (Inverse Mass + Angular part)
        // angularComponent = (I^-1 * (r x n)) x r
        Matrix4x4 invIA = bodyA.GetInverseInertiaTensorWorld();
        Matrix4x4 invIB = bodyB.GetInverseInertiaTensorWorld();

        Vector3 rAxN = Vector3.Cross(rA, normal);
        Vector3 rBxN = Vector3.Cross(rB, normal);

        Vector3 angularPartA = Vector3.Cross(invIA.MultiplyVector(rAxN), rA);
        Vector3 angularPartB = Vector3.Cross(invIB.MultiplyVector(rBxN), rB);

        float invMassSum = (1.0f / bodyA.Mass) + (1.0f / bodyB.Mass);
        float angularSum = Vector3.Dot(angularPartA + angularPartB, normal);

        j /= (invMassSum + angularSum);

        // 4. インパルスの適用
        Vector3 impulse = j * normal;

        // Aには +impulse
        bodyA.LinearVelocity += impulse * (1.0f / bodyA.Mass);
        bodyA.AngularVelocity += invIA.MultiplyVector(Vector3.Cross(rA, impulse));

        // Bには -impulse (作用・反作用)
        bodyB.LinearVelocity -= impulse * (1.0f / bodyB.Mass);
        bodyB.AngularVelocity -= invIB.MultiplyVector(Vector3.Cross(rB, impulse));

        // 5. めり込み解消 (Positional Correction)
        // これがないと、重力で沈み込んでしまう
        float percent = 0.8f; // 補正率
        float slop = 0.01f;   // 許容誤差
        float penetrationDepth = totalRadius - dist;
        Vector3 correction = (Mathf.Max(penetrationDepth - slop, 0.0f) / (1.0f/bodyA.Mass + 1.0f/bodyB.Mass)) * percent * normal;

        bodyA.Position += correction * (1.0f / bodyA.Mass);
        bodyB.Position -= correction * (1.0f / bodyB.Mass);
    }
}

描画用クラス

using UnityEngine;
using System.Collections.Generic; // Listを使うため

public class RigidBodySimulator : MonoBehaviour
{
    public GameObject SpherePrefab; // Hierarchyではなく、Project内のSphereプレハブ
    
    private List<MyRigidBody> bodies = new List<MyRigidBody>();
    private List<GameObject> visuals = new List<GameObject>();

    void Start()
    {
        // ボールA: 左から右へ
        CreateBall(new Vector3(-3, 0, 0), new Vector3(3, 0, 0)); 
        
        // ボールB: 右から左へ(少し上にずらして回転を生ませる)
        CreateBall(new Vector3(3, 0.5f, 0), new Vector3(-3, 0, 0));
    }

    void CreateBall(Vector3 pos, Vector3 vel)
    {
        float r = 1.0f;
        float m = 1.0f;
        var rb = new MyRigidBody(pos, r, m);
        rb.LinearVelocity = vel;
        bodies.Add(rb);

        var go = Instantiate(SpherePrefab, pos, Quaternion.identity);
        go.transform.localScale = Vector3.one * (r * 2); // 直径に合わせる
        visuals.Add(go);
    }

    void FixedUpdate()
    {
        float dt = Time.fixedDeltaTime;

        // 1. 力の適用と積分
        foreach (var b in bodies)
        {
            // 今回は重力なしで実験(宇宙空間での衝突)
            // b.Integrate(dt) の中に ForceAccumulatorのリセットがあるか要確認
            // 無ければ b.LinearVelocity += ... を直接書くか、Integrateを呼ぶ
            
            b.Integrate(dt); 
        }

        // 2. 衝突解決
        // 全ペア総当たり (本来は非効率だが2個ならOK)
        for (int i = 0; i < bodies.Count; i++)
        {
            for (int j = i + 1; j < bodies.Count; j++)
            {
                CollisionResolver.ResolveCollision(bodies[i], bodies[j]);
            }
        }

        // 3. 表示更新
        for (int i = 0; i < bodies.Count; i++)
        {
            visuals[i].transform.position = bodies[i].Position;
            visuals[i].transform.rotation = bodies[i].Orientation;
        }
    }
}

実際に動かしてみよう

描画用クラスを書いたスクリプトをcreate emptyで作った空のゲームオブジェクトにアタッチした後、作られた枠に前回作ったColliderコンポーネントを取り除いたsphereのプレハブを格納します。
そして動かしてみると以下のようになります。

https://youtu.be/IPE5nQqUYWY

無重力空間で、球体同士がぶつかって運動方向を変えている様子が分かりますね!

立方体同士の衝突

今回、立方体の衝突判定を計算するために分離軸判定法(SAT)という方法を用います。
これは、物体が回転しない時に用いる、毎秒

(A_{xmin}>=B_{xmax}&&A_{xmax}<=B_{xmin})&&(A_{ymin}>=B_{ymax}&&A_{ymax}<=B_{ymin})&&(A_{zmin}>=B_{zmax}&&A_{zmax}<=B_{zmin})

を調べて、それが真であれば衝突しているとして判定する手法を拡張したものです。これだけだと、回転している図形に対しての誤差が多すぎるので、以下のように計算しています。

\\(\underbrace{Overlap(\boldsymbol{A}_x) \land Overlap(\boldsymbol{A}_y) \land Overlap(\boldsymbol{A}_z)}_{\text{箱Aの3軸}}) && \\(\underbrace{Overlap(\boldsymbol{B}_x) \land Overlap(\boldsymbol{B}_y) \land Overlap(\boldsymbol{B}_z)}_{\text{箱Bの3軸}}) \\

ここで、たとえばOverlap(\boldsymbol{A}_x)は、

\mathrm{Overlap}(\boldsymbol{A}_x) = \underbrace{(\mathrm{ProjA}_{\max} \ge \mathrm{ProjB}_{\min}) \land (\mathrm{ProjB}_{\max} \ge \mathrm{ProjA}_{\min})}_{\text{軸}\boldsymbol{A}_x\text{(Aの持つx座標)にすべての頂点を投影して最大・最小を出した値}}

となります。

上記を毎秒調べてぶつかっていたら、衝突した点の特定を行います。衝突点の特定は以下のように行われています。

\min(max_A, max_B) - \max(min_A, min_B)

を、上記で挙げた箱Aの3軸と箱Bの3軸について計算する。
↓
計算した後出た値のうち最も大きさが小さいものの軸を衝突の法線ベクトルとする。(ここからは、Aの軸で衝突の法線ベクトルが出たものとして話を進めていきます。)
選ばれた法線ベクトルの向きが相手を押し出す向きである保証はありません。しかし、物理計算の \boldsymbol{n} は、必ず 「相手(B)を押し出す方向」 でなければなりません。そのため、最後に以下のチェックをします。

ベクトル \boldsymbol{d} = \text{Bの重心} - \text{Aの重心}

選ばれた軸 \boldsymbol{n} と、\boldsymbol{d} の内積をとる。もし Dot(n, d) < 0 (逆向き)だったら \boldsymbol{n} = -\boldsymbol{n} (反転させる)

Bの8つの頂点すべてについて、法線ベクトル \boldsymbol{n} との内積を計算します。

d = \boldsymbol{vertex} \cdot \boldsymbol{n}

このとき内積の値 d が 一番小さい(マイナスに大きい)頂点 が、最もAに深く入っているはずなので、求める「衝突点(Deepest Point)」です。(※内積の性質上、逆向きであればあるほど値は小さくなるからです)

SAT(分離軸判定)について

今回は簡易実装として、AとBの軸(計6本)のみを判定に使用しました。 厳密なSATの実装には、これらに加えて「AとBの各軸の外積」で生成される9本の軸も判定する必要があります(合計15軸)。 この外積軸を用いることで、辺と辺が接触しているようなケースでも正確に分離判定ができるようになります。より精度の高い物理挙動を目指す方は、ぜひ実装してみてください。

ここまでをまとめたコードを以下のように書きました。

立方体同士の衝突を扱うコード

管理クラス

using UnityEngine;

[System.Serializable]
public class MyRigidBody
{
    // --- 状態変数 (State Variables) ---
    public Vector3 Position;           // 位置 x
    public Quaternion Orientation;     // 姿勢 q (回転)
    public Vector3 LinearVelocity;     // 速度 v
    public Vector3 AngularVelocity;    // 角速度 ω

    // --- 定数・物理プロパティ (Constants) ---
    public float Mass = 1.0f;          // 質量 m
    
    public float Restitution = 0.2f;

    // --- 箱用の追加 ---
    public Vector3 HalfSize = Vector3.one * 0.5f; // 幅・高さ・奥行きの半分
    // 慣性テンソル (Local座標系での定数)
    // 直方体の場合、幅・高さ・奥行きから計算できる対角行列
    public Matrix4x4 InverseInertiaTensorLocal; 
    public float Radius = 0.5f;      // 球の半径

    // --- ワーク変数 (計算用) ---
    private Vector3 _forceAccumulator;  // 蓄積された力 F
    private Vector3 _torqueAccumulator; // 蓄積されたトルク τ

public MyRigidBody(Vector3 position, Vector3 size, float mass)
    {
        Position = position;
        HalfSize = size * 0.5f;
        Mass = mass;

        // 箱の慣性モーメント
        float w2 = size.x * size.x;
        float h2 = size.y * size.y;
        float d2 = size.z * size.z;
        Vector3 I = new Vector3(
            (mass / 12.0f) * (h2 + d2),
            (mass / 12.0f) * (w2 + d2),
            (mass / 12.0f) * (w2 + h2)
        );

        InverseInertiaTensorLocal = Matrix4x4.identity;
        InverseInertiaTensorLocal[0, 0] = 1.0f / I.x;
        InverseInertiaTensorLocal[1, 1] = 1.0f / I.y;
        InverseInertiaTensorLocal[2, 2] = 1.0f / I.z;
        InverseInertiaTensorLocal[3, 3] = 1.0f;
    }

    // ワールド空間での軸を取得するヘルパー
    public Vector3 GetAxis(int index) 
    {
        // 0:Right, 1:Up, 2:Forward
        Matrix4x4 R = Matrix4x4.Rotate(Orientation);
        return R.GetColumn(index); // 行列の列ベクトルがそのままローカル軸のワールド表現
    }

    // 頂点群をワールド座標で取得
    public Vector3[] GetVerticesWorld()
    {
        Vector3[] vertices = new Vector3[8];
        Vector3[] corners = {
            new Vector3(1, 1, 1), new Vector3(1, 1, -1), new Vector3(1, -1, 1), new Vector3(1, -1, -1),
            new Vector3(-1, 1, 1), new Vector3(-1, 1, -1), new Vector3(-1, -1, 1), new Vector3(-1, -1, -1)
        };
        
        Matrix4x4 trs = Matrix4x4.TRS(Position, Orientation, Vector3.one);
        for(int i=0; i<8; i++)
        {
            // Vector3.Scaleでサイズを適用してから回転・移動
            vertices[i] = trs.MultiplyPoint3x4(Vector3.Scale(corners[i], HalfSize));
        }
        return vertices;
    }
    // 力を加えるメソッド (重心以外の点に加えると回転が生じる)
    public void AddForceAtPoint(Vector3 force, Vector3 point)
    {
        _forceAccumulator += force;

        // 重心(Position)から力点(point)へのベクトル r
        Vector3 r = point - Position;

        // トルク τ = r × F (外積)
        _torqueAccumulator += Vector3.Cross(r, force);
    }

    // 積分ステップ (物理世界の時間を進める)
    public void Integrate(float dt)
    {
        // 1. 並進運動の更新 (F = ma)
        // 加速度 a = F / m
        Vector3 acceleration = _forceAccumulator / Mass;
        LinearVelocity += acceleration * dt;
        Position += LinearVelocity * dt;

        // 2. 回転運動の更新 (τ = I * dω/dt)
        // ここが難しいポイント:慣性テンソルは「回っている物体」と一緒に回るため、
        // World座標系での慣性テンソル逆行列を毎フレーム計算し直す必要がある。
        // I_world^-1 = R * I_local^-1 * R^T (Rは回転行列)
        
        Matrix4x4 R = Matrix4x4.Rotate(Orientation); // クォータニオンから回転行列へ
        Matrix4x4 InverseInertiaTensorWorld = R * InverseInertiaTensorLocal * R.transpose;

        // 角加速度 α = I^-1 * τ
        // Matrix4x4.MultiplyVector で行列とベクトルの掛け算
        Vector3 angularAcceleration = InverseInertiaTensorWorld.MultiplyVector(_torqueAccumulator);
        
        AngularVelocity += angularAcceleration * dt;

        // ドラッグ(空気抵抗的な減衰)を入れないと無限に回転し続けるので少し入れる
        AngularVelocity *= 0.99f; 

        // 3. 姿勢(クォータニオン)の更新
        // dq/dt = 0.5 * ω * q
        // 小さい回転を加算する処理
        Quaternion deltaQ = new Quaternion(
            AngularVelocity.x * dt * 0.5f,
            AngularVelocity.y * dt * 0.5f,
            AngularVelocity.z * dt * 0.5f,
            0 // w成分は近似的に0で扱う(微小回転のため)
        );
        
        // クォータニオンの掛け算で回転を合成
        // 注意: 単純な足し算ではなく、今の姿勢に微小回転を掛ける
        // ここでは実装簡易化のため、Unityの内部計算に近い形「q_new = q + (0.5 * w * q * dt)」を簡略化した以下を採用
        //↑微分の定義より近似
        
        // ωをクォータニオン化して合成する一般的な手法
        // q_new = q_old + (dq/dt * dt)
        Quaternion qDot = deltaQ * Orientation; 
        
        Orientation.x += qDot.x;
        Orientation.y += qDot.y;
        Orientation.z += qDot.z;
        Orientation.w += qDot.w;

        // 正規化(大きさを1に保たないと計算誤差で形が歪む)
        Orientation.Normalize();

        // 4. 力をリセット
        _forceAccumulator = Vector3.zero;
        _torqueAccumulator = Vector3.zero;
    }

    // ワールド空間での逆慣性テンソルを取得するヘルパー
    public Matrix4x4 GetInverseInertiaTensorWorld()
    {
        Matrix4x4 R = Matrix4x4.Rotate(Orientation);
        return R * InverseInertiaTensorLocal * R.transpose;
    }
}

衝突クラス

using UnityEngine;

public static class BoxCollisionResolver
{
    // 衝突情報構造体
    struct CollisionInfo
    {
        public bool HasCollision;
        public float Penetration;
        public Vector3 Normal; // AからBへのベクトル
    }

    public static void ResolveCollision(MyRigidBody boxA, MyRigidBody boxB)
    {
        CollisionInfo col = CheckSAT(boxA, boxB);
        if (!col.HasCollision) return;

        // --- 衝突点の特定 (Deepest Point) ---
        // 衝突法線を使って、どちらが「ぶつかってきた側(Incident)」か判定
        // Bの頂点のうち、Aの中心に一番近い(あるいはAの面に一番深く刺さっている)点を探す簡易実装
        
        Vector3 contactPoint = Vector3.zero;
        float minProj = float.MaxValue;
        
        // Bの全頂点をAのローカル空間あるいは法線方向で評価
        Vector3[] vertsB = boxB.GetVerticesWorld();
        // 衝突法線の逆方向(めり込んでいる方向)にある頂点を探す
        foreach(var v in vertsB)
        {
            float proj = Vector3.Dot(v - boxA.Position, col.Normal);
            if(proj < minProj)
            {
                minProj = proj;
                contactPoint = v;
            }
        }
        
        // もしAの頂点がBに入っている場合もあるので、本来は双方向チェックが必要ですが、
        // 今回はSATの法線ベクトルが「AからBへ押し出す」向きになっているため、
        // Bの頂点がAに入っていると仮定して処理を進めます。

        // --- インパルス計算 (球と同じ) ---
        ApplyImpulse(boxA, boxB, contactPoint, col.Normal, col.Penetration);
    }

    // SATの実装
    static CollisionInfo CheckSAT(MyRigidBody A, MyRigidBody B)
    {
        float minPenetration = float.MaxValue;
        Vector3 bestAxis = Vector3.zero;

        // チェックする軸のリスト(Aの3軸 + Bの3軸)
        Vector3[] axesToTest = new Vector3[6];
        for(int i=0; i<3; i++) axesToTest[i] = A.GetAxis(i);
        for(int i=0; i<3; i++) axesToTest[3+i] = B.GetAxis(i);

        foreach(var axis in axesToTest)
        {
            if (axis == Vector3.zero) continue;

            // 軸への投影幅を計算
            float pA = GetProjectedRadius(A, axis);
            float pB = GetProjectedRadius(B, axis);

            // 中心距離の投影
            float distance = Mathf.Abs(Vector3.Dot(B.Position - A.Position, axis));

            // 重なり = (半径A + 半径B) - 中心距離
            float overlap = (pA + pB) - distance;

            if (overlap < 0)
            {
                // 1つでも重なっていない軸があれば、衝突していない
                return new CollisionInfo { HasCollision = false };
            }

            // 最も浅い重なりを探す(そこが脱出経路)
            if (overlap < minPenetration)
            {
                minPenetration = overlap;
                bestAxis = axis;
            }
        }

        // 法線の向きを整える(常にBからAへ向くように)
        if (Vector3.Dot(bestAxis, B.Position - A.Position) > 0)
        {
            bestAxis = -bestAxis;
        }

        return new CollisionInfo
        {
            HasCollision = true,
            Penetration = minPenetration,
            Normal = bestAxis
        };
    }

    // 箱を軸に投影したときの「半径(中心から端までの長さ)」を計算
    static float GetProjectedRadius(MyRigidBody box, Vector3 axis)
    {
        // 箱の「影」の長さは、各軸成分の投影の和になる
        // R = |dot(x, axis)| * halfX + |dot(y, axis)| * halfY + ...
        return 
            Mathf.Abs(Vector3.Dot(box.GetAxis(0), axis)) * box.HalfSize.x +
            Mathf.Abs(Vector3.Dot(box.GetAxis(1), axis)) * box.HalfSize.y +
            Mathf.Abs(Vector3.Dot(box.GetAxis(2), axis)) * box.HalfSize.z;
    }

    // インパルス応答(球のときと全く同じ式です)
    static void ApplyImpulse(MyRigidBody A, MyRigidBody B, Vector3 contactPoint, Vector3 normal, float penetration)
    {
        Vector3 rA = contactPoint - A.Position;
        Vector3 rB = contactPoint - B.Position;

        Vector3 velA = A.LinearVelocity + Vector3.Cross(A.AngularVelocity, rA);
        Vector3 velB = B.LinearVelocity + Vector3.Cross(B.AngularVelocity, rB);
        Vector3 relVel = velA - velB;

        float velAlongNormal = Vector3.Dot(relVel, normal);
        if (velAlongNormal > 0) return;

        float e = Mathf.Min(A.Restitution, B.Restitution);
        float j = -(1 + e) * velAlongNormal;

        Matrix4x4 invIA = A.GetInverseInertiaTensorWorld();
        Matrix4x4 invIB = B.GetInverseInertiaTensorWorld();

        Vector3 rAxN = Vector3.Cross(rA, normal);
        Vector3 rBxN = Vector3.Cross(rB, normal);

        float angularTerm = Vector3.Dot(Vector3.Cross(invIA.MultiplyVector(rAxN), rA) + 
                                        Vector3.Cross(invIB.MultiplyVector(rBxN), rB), normal);
        
        float invMassSum = (1.0f / A.Mass) + (1.0f / B.Mass);
        j /= (invMassSum + angularTerm);

        Vector3 impulse = j * normal;

        A.LinearVelocity += impulse * (1.0f / A.Mass);
        A.AngularVelocity += invIA.MultiplyVector(Vector3.Cross(rA, impulse));
        B.LinearVelocity -= impulse * (1.0f / B.Mass);
        B.AngularVelocity -= invIB.MultiplyVector(Vector3.Cross(rB, impulse));

        // 位置補正
        float percent = 0.5f; 
        Vector3 correction = normal * (penetration * percent);
        A.Position += correction * 0.5f;
        B.Position -= correction * 0.5f;
    }
}

描画用クラス

using UnityEngine;
using System.Collections.Generic;

public class BoxSimulator : MonoBehaviour
{
    public GameObject CubePrefab;
    private List<MyRigidBody> bodies = new List<MyRigidBody>();
    private List<GameObject> visuals = new List<GameObject>();

    void Start()
    {
        // 箱A: 静止
        CreateBox(new Vector3(0, 0, 0), Vector3.zero, new Vector3(0,0,0)); 

        // 箱B: 回転しながら突っ込む
        var b = CreateBox(new Vector3(0, 4, 0.5f), new Vector3(0, -2, 0), new Vector3(10, 5, 5));
    }

    MyRigidBody CreateBox(Vector3 pos, Vector3 vel, Vector3 angularVel)
    {
        Vector3 size = new Vector3(1, 1, 1);
        MyRigidBody rb = new MyRigidBody(pos, size, 1.0f);
        rb.LinearVelocity = vel;
        rb.AngularVelocity = angularVel; // 初速で回転を与える
        
        bodies.Add(rb);

        var go = Instantiate(CubePrefab, pos, Quaternion.identity);
        go.transform.localScale = size;
        visuals.Add(go);
        return rb;
    }

    void FixedUpdate()
    {
        float dt = Time.fixedDeltaTime;

        // 積分
        foreach (var b in bodies) b.Integrate(dt);

        // 衝突判定
        if(bodies.Count >= 2)
            BoxCollisionResolver.ResolveCollision(bodies[0], bodies[1]);

        // 描画
        for(int i=0; i<bodies.Count; i++)
        {
            visuals[i].transform.position = bodies[i].Position;
            visuals[i].transform.rotation = bodies[i].Orientation;
        }
    }
}

実際に動かしてみよう

描画用クラスを書いたスクリプトをcreate emptyで作った空のゲームオブジェクトにアタッチした後、作られた枠にColliderコンポーネントを取り除いたcubeのプレハブを格納します。
そして動かしてみると以下のようになります。

https://youtu.be/laFSfn12-KE

無重力空間で、回転しながら落ちてくる立方体と静止している立方体がぶつかって運動方向そして回転方向を変えている様子が分かりますね!

おわりに

ということで、物理エンジンを自力で作る企画今回は球体同士の衝突・立方体同士の衝突まで扱いました。次回は摩擦と複数点衝突などを扱うので、ぜひ楽しみにしていてください。

Part3はこちら

Discussion