はじめに

予めお断りしておきますが、タイトルにもあるように今回は失敗談です。
詳細は後述しますが、目的に合ったものはできませんでした。

それでも記事を書いたのは、誰かにアドバイスがもらえる可能性に期待したのと、失敗から得た経験について書きたかったのと、もしかしたら違う用途ならば誰かの参考にしてもらえる部分もあるのではないか、という~~悔し紛れの~~思いからです。

#経緯

コンピュートシェーダー(ComputeShader)を学ぶため、自動車を動かす交通シミュレーターもどきを作ってみようと思いました。個々の自動車がそれぞれ衝突を回避しつつ適切な経路で目的地に移動できるようになるのが目標です。
前回でようやく道路上を車を模したポリゴンを動かすことはできました。しかしただ直進するだけで、何もシミュレーターっぽいことはできてません。

今回はいよいよ、他の車との衝突を予測して、必要に応じて減速するような仕組みを作ってみます。なお、コードはいままでの分も含め全てGithubにあります。

◀【その３：車に道路上を走らせる+パーティクル版】
【その5：衝突予測・リベンジ】▶

結果

今回、失敗談ということもあって、先に何が出来て何が出来なかったか報告します。

まず、対向車線もない完全単車線なら、こんな風に衝突回避はできました。

しかし複数車線にすると、別車線の車にも反応してしまいました。

パラメーター調整によって感度を減らすことも可能ですが、そうすると同じ車線の車に対しても、衝突ギリギリまで接近しないと回避判断ができません。

他にも車の追加投入時から回避判定が働いて、徐々に入口で詰まってしまうバグもあり、残念ながら目的達成には程遠い結果となりました。

C#側実装

【CarController04の全体はこちら】

構造体の変更

まずはアルゴリズムで使用する（つもり）のパラメーターを増やします。

CarTemplate03.cs

/// <summary>
/// 車の構造体
/// </summary>
public struct Car03d : ICarDynamicInfo
{
    public Vector2 pos { get; set; }
    public Vector2 direction { get; set; }
    public float velocity { get; set; }
    public int colider { get; set; } // 衝突の可能性が最も高い相手のindex
}

/// <summary>
/// 車の構造体
/// </summary>
public struct Car03s : ICarStaticInfo
{
    public Vector3 size { get; set; }
    public Color color { get; set; }
    public float idealVelocity { get; set; } // 目標速度
    public float mobility { get; set; } // 機動性(加減速度合い)
}

車の生成と消滅の制御

とりあえず出現した時点で他の車と重なってしまっているようだと、まともに回避判定の評価ができないので、まずは車を一定間隔で投入できるように変更しました。

CarSController4

    void InitializeComputeBuffer()
    {
        factory = new CarRepository(MAX_CARS, CarTemplate03.dictionary);
        factory.AssignBuffers();


        StartCoroutine(WatchLoop(OnEachScan, OnEachElement));

        factory.ApplyData();
    }

    const int MIN_INTERVAL_FRAMES = 90;
    private Dictionary<int, int> _lastEntries = new Dictionary<int, int>();

    private void OnEachScan(Car[] cars)
    {
        if (factory.ActiveCars >= MAX_CARS) return;

        var entries = roadPlane.EntryPoints;

        // 車を追加する
        int index = Random.Range(0, entries.Count);
        int f;
        if( _lastEntries.TryGetValue(index, out f) && Time.frameCount - f < MIN_INTERVAL_FRAMES)
        { // 最初から衝突するのを避けるため、一定フレーム数内に車が入ったばかりなら次の機会を待つ
            return; 
        }

        var entry = entries[index];
        factory.CreateRandomType(entry.pos, entry.dir);
        _lastEntries[index] = Time.frameCount;
        factory.ApplyData();
    }

    private void OnEachElement(int index, Car car)
    {
        if ( roadPlane.isInside(car.Dynamic.pos) ) return;

        factory.Remove(index);
        factory.ApplyData();
    }

    private int _currentIndex = 0;
    private IEnumerator WatchLoop(Action<Car[]> onScan, Action<int, Car> onElement)
    {
        // １フレーム中に進むカウント数
        const int COUNT_PER_FRAME = 10;
        Car[] carsArray = factory.GetCars();
        onScan(carsArray);
        while (true) {

            int cars = factory.ActiveCars;
            if (cars > 0)
            {
                if (_currentIndex >= cars)
                {
                    _currentIndex = 0;
                    carsArray = factory.GetCars();
                    onScan(carsArray);
                    yield return null;
                    continue;
                }
                var car = carsArray[_currentIndex];
                onElement(_currentIndex, car);
                _currentIndex++;

                // 
                if (_currentIndex % COUNT_PER_FRAME == 0)
                {
                    carsArray = factory.GetCars();
                    onScan(carsArray);
                    yield return null;
                    continue;
                }
            }
        }
    }

車の追加と削除は C# 側から制御したかったのですが、毎フレームバッファを全部チェックするのは負荷が高くなりすぎるし、そもそもそんな精度は求めていないので、コルーチンで毎フレーム一定数だけ走査するようにしてみました。なので、上のスクショを見てもわかりますが、道路の範囲をはみ出ても削除されるまでに若干のタイムラグがあります。

衝突回避アルゴリズム(シェーダー側)

今回は衝突予測の部分がメインです。
回避というからには、ハンドル操作で避けて進みたいところですが、難しそうなので今回は減速するのみにしたいと思います。
【全体のソースはこちら】

ちなみに以下のロジックは、ほぼ忘れかけてた高校数学の知識を必死に思いつつ、無い知恵絞って捻りだしたものです。もしも間違いなどあれば訂正していただけたら幸いです。

衝突予測

二つの移動する点が将来衝突するかどうか判定するには、両者の相対位置ベクトルと相対速度ベクトルが同じ方向を向いているかどうか調べればよいでしょう。
点と点との衝突なら方向が完全一致する場合だけ考えればよいですが、ある程度サイズを持った物体同士の場合は、完全に一致しない場合でも衝突の可能性はあります。

図1.衝突する相対位置と速度	図2.衝突しない相対位置と速度

ただし少なくとも、両ベクトルが90度以上開いていれば（すでに衝突している状態でなければ）このまま直進する限り衝突することはないと言えそうです。つまり、相対位置ベクトルと相対速度ベクトルの内積が０以下なら、衝突の可能性は排除できます。逆に、０より大きい場合はより詳細な確認が必要になります。

図３.相対位置と速度の角度と衝突可能性


  CarD carD1 = CarsDynamic[id1];
  CarD carD2 = CarsDynamic[id2];
  // 相対位置ベクトル
  float2 diffPos = carD2.pos - carD1.pos;
  // 相対速度ベクトル
  float2 diffVel = carD1.dir * carD1.velocity - carD2.dir * carD2.velocity;

  float dotPosAndVel = dot2d(diffPos,diffVel);
  // 内積が０以下なら衝突の可能性なし！
  if(dotPosAndVel < 0) return;

次に内積が0より大きい場合について。

まず、後方から接近してくる場合は、後続車側が適切に回避してくれることを期待して特に何もしないことにします。実際後方からの接近について回避するような運転は現実でも滅多に見られないでしょう。

  // 背後から接近してくるものは回避しない（相手任せ）
  if (dot2d(diffPos, carD1.dir) < 0) return;

さて、もし二つの車がサイズを持たない点だったら、このまま直進した時に二点が最接近するのは、相対位置ベクトルから相対速度ベクトルに垂線を降ろした点Pになります（図4）。
（ただし絶対座標ではなく、B車を基準にしたローカル座標です）

図4. 最接近点

車がサイズを持つ場合、線分BPの長さが「二つの車のサイズを考慮した距離」以下なら衝突すると見なせると思います。

また、線分APを相対速度（の絶対値）で割れば、衝突までにかかる時間がわかります。線分APの長さは相対位置ベクトルと相対速度ベクトルの内積で求められるはずです。

ただし、このままだと衝突するとしても、それが遠い未来の話なら、処理を効率化するためにも一旦衝突はないと判断してよさそうです。そうでないなら、減速などの回避行動を行うようにしたいと思います。

  float absVel = length(diffVel);
  if(absVel < 0.001) return; // 接近していない

  float countAssume = dotPosAndVel / absVel;
  if(countAssume > MAX_FORCAST_COUNT) return; // 遠い未来過ぎるので無視

  // 二つの車のサイズを考慮した距離を求める
  CarS carS1 = CarsStatic[id1];
  CarS carS2 = CarsStatic[id2];
  float2 diffVelNml = normalize(diffVel);
  float d1,d2;
  GetSafeDistance(carS1, carD1, diffVelNml, d1);
  GetSafeDistance(carS2, carD2, -diffVelNml, d2);
  float distance = d1 + d2; 

  // 最接近点での距離が二つの車のサイズを考慮した距離以上なら衝突しない
  float crossPosAndVel = abs(cross2d(diffPos, diffVelNml));
  if(crossPosAndVel > distance) return;

では「二つの車のサイズを考慮した距離」はどう考えるか。

二つの車がお互い相手側に最も張り出した頂点がわかれば、そこで接した時が二つの車が衝突しうる最大距離になります（図5a）。
車の基準点（相対位置を計算する際の点）と「相手側に最も近い頂点」を結ぶベクトル(s, t)を合成したベクトル u の絶対値で計算できそうです。

図5a. サイズを考慮した最大衝突距離	図5b. 頂点同士以外での衝突

もちろん実際に衝突するときは図5bのように頂点以外の部分と接触することのほうが多いですが、それは必ず図5aの距離よりも近いはず。
これがもし衝突判定なら、さらに精密な計算が必要ですが、今回は衝突判定がしたいわけではなく、衝突を予測して回避がしたいだけなので、紙一重でギリギリかわせるようなところを狙う必要はなく、「疑わしきは回避」すればよいと思います。

最大衝突距離 u 以上に接近しなければ安全と言えるので、これを「二つの車のサイズを考慮した距離」とし、図4の線分BPの長さがそれより大きければ衝突はしないと判断できます。

一方、そうでない場合はより詳細な衝突予測をして、回避行動を行うようにしたいですね。
上の方で、「図4の線分APの長さから衝突までにかかる時間がわかる」と書きましたが、図5を見てわかるように車のサイズを考慮すれば、最接近点Pより前に衝突することになります。
したがって図6のように線分APからベクトル u の長さを引いたうえで（＝線分AQ）、衝突までの時間を計算するのがよさそうです。

図6. 安全な衝突予想距離

  // 最接近点での距離が二つの車のサイズを考慮した距離以上なら衝突しない
  float crossPosAndVel = abs(cross2d(diffPos, diffVelNml));
  if(crossPosAndVel > distance) return;

  // このままだと近い将来衝突しそう

  float t = max(0, (dotPosAndVel - distance) / absVel);
  if(t > timeMin) return; // もっと近い相手が既にいる

  // 最小値更新
  timeMin = t;
  idMin = id2;

最後に、「相手側に最も近い頂点」はどうやって判断するか考えてみます。
車の基準点（必ずしも幾何学的中心でなくてもいい）から各頂点へのベクトルと、相対位置ベクトルの内積が最大になるのが、相手に最も近い頂点と言えそうです（図7）。

図7. 相手側に最も近い頂点

// 2Dベクトル回転
inline float2 rotate2d(in float2 v2d, in float2x2 mat)
{
  return mul(v2d, mat);
}

// 指定方向に最も張り出した頂点の距離から、最大衝突距離を計算する
inline void GetSafeDistance(in CarS carS, in CarD carD, in float2 diffPosNml, out float distance){
  // Y軸回転の行列を作る
  float sina = cross2d(carD.dir, diffPosNml);
  float cosa = dot2d(carD.dir, diffPosNml);
  float2x2 _matrix = float2x2(cosa, -sina, sina, cosa);

  // 車の四方の頂点を評価して、最大値を保存する
  float2 vert = carS.size.xz * 0.5; // (x,y)
  // dir は (1,0) が進行方向になっているので、回転後のy座標が diffPosNml との内積に等しい
  distance = rotate2d(vert, _matrix).y;
  vert *= -1; // (-x,-y)
  distance = max(distance, rotate2d(vert, _matrix).y);
  vert.x *= -1; // (x,-y)
  distance = max(distance, rotate2d(vert, _matrix).y);
  vert *= -1; // (-x,y)
  distance = max(distance, rotate2d(vert, _matrix).y);
}

回避および加速処理

総当たりで最も危険な他車を見つけ、見つかったら減速します。
見つからなかった場合、現在の速度が理想速度以下なら、加速します。

[numthreads(8,1,1)]
void CSMain (uint3 id : SV_DispatchThreadID)
{
  uint idMin = id.x;
  int timeMin = 1000; // このままだとあと何回で衝突するか
  
  for(uint i = 0; i < count; i++) {
    if(i == id.x) continue;
    if(CarsStatic[i].size.z == 0) break; // 削除済みのデータ＝末端に到達
    FindMostDangerCar(id.x, i, timeMin, idMin);
  }
  
  CarD carD = CarsDynamic[id.x];
  CarS carS = CarsStatic[id.x];

  if(idMin == id.x) { // 衝突の可能性の高い車はない
    if(carD.velocity < carS.idealVelocity) {
      carD.velocity = min(carD.velocity + carS.mobility, carS.idealVelocity);
    }
  }
  else {
    if(carD.velocity > 0) {
      carD.velocity = max(0,  carD.velocity - carS.mobility * 500 / max(0.1, timeMin));
      if( length(CarsDynamic[idMin].pos - carD.pos) < 500 ){
        carD.velocity = 0;
      }
    }
  }

  // それぞれの位置情報に移動ベクトルを加算　(0.28はkm/hをm/sに変換する係数)
  carD.pos += carD.dir * carD.velocity * DeltaTime * 0.28;
  CarsDynamic[id.x] = carD;
}

本当は、もっと滑らかに減速させたかったのですが、衝突予測のバグがなくなるまでは、簡単のために一旦その場で完全停止するようにしました。

それと for(uint i = 0; i < count; i++) で総当たりはイケてないですよね。
折角三次元のスレッドが可能なのだから、[numthreads(8,8,1)] とかで回したいですね。その場合はPassも予測と回避の二段階必要でしょうが。
この辺は次回の宿題としたいと思います。

ComputeShader のデバッグに苦しむ

今回気付かされた一番の点は、シェーダーのデバッグが難しいということです。
なにしろ、デバッガーをアタッチしてブレークポイントで止めたりステップ実行したりできないし、デバッグログを出すこともできない。~~まだカーネルデバッグの方がマシなレベル~~。

それでも通常のフラグメントシェーダーとかなら、ステップ実行したりするツールなどあるようですが、コンピュートシェーダーの計算過程を調べる方法はほぼないようでした。（データ構造を変えて結果として出力させたりすることはできますが、非常に手間がかかる。）

結局、脳内デバッグだけでは埒が明かないと判断して、一旦コンピュートシェーダーを全部C#で書き直すことにしました。

下記は書き直した衝突予想ロジックのメソッドです。

CarsController04.driving.cs

    // 最も衝突の可能性の高い車のid(index)を返す
    private void FindMostDangerCar(Car[] cars, int id1, int id2, ref float timeMin, ref int idMin)
    {
        if (timeMin <= 0) return; // 時すでに遅し

        var carD1 = cars[id1].Dynamic;
        var carD2 = cars[id2].Dynamic;
        // 相対位置ベクトル
        Vector2 diffPos = carD2.pos - carD1.pos;
        // 相対速度ベクトル
        Vector2 diffVel = carD1.direction * carD1.velocity - carD2.direction * carD2.velocity;

        float dotPosAndVel = dot2d(diffPos, diffVel);
        // 内積が０以下なら衝突の可能性なし！
        if (dotPosAndVel < 0)
        {
            return;
        }

        // 背後から接近してくるものは回避しない（相手任せ）
        if (dot2d(diffPos, carD1.direction) < 0)
        {
            return;
        }

        float absVel = diffVel.magnitude;
        if (absVel < 0.001f) return; // 接近していない

        float countAssume = dotPosAndVel / absVel;
        if (countAssume > MAX_FORCAST_COUNT)
        {
            return; // 遠い未来過ぎるので無視
        }

        // 二つの車のサイズを考慮した距離を求める
        var carS1 = cars[id1].Static;
        var carS2 = cars[id2].Static;
        Vector2 diffVelNml = diffVel.normalized;
        Vector2 diffPosNml = diffPos.normalized;
        float d1, d2;
        GetSafeDistance(carS1, carD1, diffVelNml, out d1);
        GetSafeDistance(carS2, carD2, -diffVelNml, out d2);
        float distance = d1 + d2;

        // 最接近点での距離が二つの車のサイズを考慮した距離以上なら衝突しない
        float crossPosAndVel = Mathf.Abs(cross2d(diffPos, diffVelNml));
        if (crossPosAndVel > distance)
        {
            return;
        }

        // どちらかが高速で移動しているなら停止距離には余裕を持つ
        //distance += Mathf.Max(carD2.velocity, carD1.velocity) * 0.28f;

        float t = Mathf.Max(0, (dotPosAndVel - distance) / absVel);
        // このままだと近い将来衝突しそう
        if (t > timeMin)
        {
            return; // もっと近い相手が既にいる
        }        

        // 最小値更新
        timeMin = t;
        idMin = id2;
        if(carD1.direction.y >= 0)
        Debug.Log(
            string.Format("<color='grey'>{0}<->{1}:\n</color> t0={2:0.0}, v={3:0.0}, dist={4:0.0}+{5:0.0}, t={6:0.0}({7:0.0})",
                DebugStr(id1, cars[id1]), DebugStr(id2, cars[id2]), countAssume, absVel, d1, d2, t, timeMin));

    }

ファイル全体のソースはこちら

まあ、もともとシェーダーでできることは限られているので移植はそんなに大変ではないです。froat2,3,4 を Vector2,3,4 に変えたり、ビルトイン関数を Mathf の関数や Vectorなどのメソッドに置き換えるだけです。
二次元回転だけはC#に2x2行列がないので、若干手間がかかりましたが・・・。

CarsController04.driving.cs

    // 2Dベクトル回転
    private Vector2 rotate2d(Vector2 v2d, Quaternion mat)
    {
        return mat * v2d;
    }

    // 指定方向に最も張り出した頂点の距離から、最大衝突距離を計算する
    private void GetSafeDistance(Car_S carS, Car_D carD, Vector2 diffPosNml, out float distance)
    {
        // Y軸回転の行列を作る
        Quaternion _matrix = Quaternion.FromToRotation(diffPosNml, carD.direction);

        // 車の四方の頂点を評価して、最大値を保存する
        Vector2 vert = new Vector2(carS.size.x, carS.size.z) * 0.5f; // (x,y)
                                                                     // dir は (1,0) が進行方向になっているので、回転後のy座標が diffPosNml との内積に等しい
        distance = rotate2d(vert, _matrix).y;
        vert *= -1; // (-x,-y)
        distance = Mathf.Max(distance, rotate2d(vert, _matrix).y);
        vert.x *= -1; // (x,-y)
        distance = Mathf.Max(distance, rotate2d(vert, _matrix).y);
        vert *= -1; // (-x,y)
        distance = Mathf.Max(distance, rotate2d(vert, _matrix).y);
    }

CPUとGPUを簡単に切り替えられるようにデバッガーシンボルを定義して、かつメニューから簡単に切り替えられるようにもしました。

これで、かなりデバッグが楽になりました。実際、ソース眺めてるだけでは気づかなかったバグをいくつか見つけられましたし、各段階での計算値が想定通りかも確認できました。
ただ、ここまでしても、微妙にシェーダーと挙動が異なってる部分があって参りますが・・・。

まとめと反省

コンピュートシェーダーで衝突予測と適切な減速処理を試みました。
しかし、他車線と同一車線の両方に対して予測判定をうまく調節することができませんでした。

一応、「接近する物体を検知する」ことは成功していると考えています。ただ、それが車のように車線に沿ってを走ることを想定した場合、例えばリアルでは対向車ははみ出してこないと互いに信頼しているので、減速しないのだと思います。そういった部分を組み込むことが出来なかったのが失敗だと考えています。

コンピュートシェーダーのデバッグは難しいことがわかりました。
複雑なロジックを書くときは、先にC#などでしっかりデバッグしてから移植するのがよいと思いました。

ロジックの不備やコードのバグなど、何でもお気づきの点があれば、コメントで指摘していただけたらありがたいです。