【Unity】衝突判定をゼロから実装 -球とカプセル-

ゼロから実装する衝突判定シリーズ第二弾、球とカプセルの衝突判定です。

考え方

前提として、カプセルを構成する二つの半球の中心をそれぞれ {P_s} {P_e}、半球の半径を {r'}とします。
球の中心は {P_c}、球の半径は {r}とします。

f:id:halya_11:20180903234635p:plain:w400

これらの情報から衝突判定を計算していきます。

最初に、 {P_s}から {P_e}に向かう単位ベクトルと、 {P_e}から {P_c}へのベクトルとの内積を取ります。
ベクトルの性質により、 {P_s}to  {P_e}を通る線上に {P_c}から垂線を下ろした時の点 {P_n}が求められます。

f:id:halya_11:20180903235233p:plain:w400

次に、上で求めたPnを使って次の式が表す値を求めます。

 { \displaystyle
\frac{P_n-P_s}{P_e-P_s}
}

この値を用いると以下の判定が行えることは自明です。

  1. この値が0〜1であれば、 {P_c} {P_s} {P_e}がなす線分に垂線を下ろせる位置にある
  2. この値が1より大きければ、 {P_c} {P_s} {P_e}がなす線分に垂線を下ろせない、かつ  {P_s}より {P_e}に近い位置にある
  3. この値が0未満であれば、 {P_c} {P_s} {P_e}がなす線分に垂線を下ろせない、かつ {P_e}より {P_s}に近い位置にある

f:id:halya_11:20180904000128p:plain:w400

次にこれら3つの条件ごとに最短距離を求めます。

1.の場合、最短距離は {P_c-P_n}の長さから { r' }を引いたものになります。
2.の場合、最短距離は {P_c-P_e}の長さから { r' }を引いたものになります。
3.の場合、最短距離は {P_c-P_s}の長さから { r' }を引いたものになります。

f:id:halya_11:20180904000906p:plain:w400

最短距離が求められたらあとは球とカプセルの半径を用いて衝突判定をするだけです。
 { r }よりも最短距離が小さければ衝突していることになります。

実装の前提

以前、球と球との当たり判定に関する記事を書きました。

light11.hatenadiary.com

下記のインタフェイスとクラスは上記の記事のものを流用します。

  • ICollider: コライダー用のインタフェイス
  • SampleCollideDetector: 毎フレームISphereとIColliderの衝突判定を行い、衝突していたら赤いSphereGizmoに描画する

実装

カプセル型のコライダーを作ります。

using UnityEngine;

public class ColliderCapsule : MonoBehaviour, ICollider {

    /// <summary>
    /// 軸の方向
    /// 値はベクトルにしたときに1を代入する要素のindexを示す
    /// </summary>
    public enum Direction
    {
        XAxis   = 0,
        YAxis   = 1,
        ZAxis   = 2,
    }
    
    /// <summary>
    /// カプセルを構成する半球の半径
    /// </summary>
    [SerializeField]
    private float       _radius     = 0.5f;
    /// <summary>
    /// 中心のローカル座標
    /// </summary>
    [SerializeField]
    private Vector3     _center;
    /// <summary>
    /// ローカル座標においてカプセルが伸びる方向
    /// </summary>
    [SerializeField]
    private Direction   _direction  = Direction.YAxis;
    /// <summary>
    /// カプセルの空間における高さ
    /// </summary>
    [SerializeField]
    private float       _height     = 2.0f;
        
    private Transform _transform;

    private void Awake()
    {
        _transform = transform;
    }

    /// <summary>
    /// 球との当たり判定
    /// </summary>
    public bool CheckSphere(ISphere sphere)
    {
        // カプセルの空間との変換行列
        var forward         = Vector3.zero;
        forward[(int)_direction] = 1.0f;
        var casuleToLocal   = Matrix4x4.TRS(_center, Quaternion.LookRotation(forward), Vector3.one);
        var worldToCapsule  = casuleToLocal.inverse * _transform.worldToLocalMatrix;

        // カプセルの空間における球の中心
        var sphereCenter    = worldToCapsule.MultiplyPoint(sphere.WorldCenter);
        
        // カプセルを構成する二つの半球の中心座標
        var end             = new Vector3(0.0f, 0.0f, 1.0f) * (_height - _radius * 2.0f) / 2.0f;
        var start           = end * -1.0f;

        // 二つの球をつなぐ線分との近傍点を求める
        var startToSphere   = sphereCenter - start;
        var startToEnd      = end - start;
        var nearLength      = Vector3.Dot(startToEnd.normalized, startToSphere);
        var nearLengthRate  = nearLength / startToEnd.magnitude;
        var near            = start + startToEnd * Mathf.Clamp01(nearLengthRate);

        // 球とカプセルとの最短距離を求める
        var sqrDistance     = 0.0f;
        var endToSphere     = sphereCenter - end;
        var nearToSphere    = sphereCenter - near;
        if (nearLengthRate < 0) {
            // 近傍点が線分上になく、start寄りにある場合
            sqrDistance = startToSphere.sqrMagnitude;
        }
        else if (nearLengthRate > 1) {
            // 近傍点が線分上になく、end寄りにある場合
            sqrDistance = endToSphere.sqrMagnitude;
        }
        else {
            // 近傍点が線分上にある場合
            sqrDistance = nearToSphere.sqrMagnitude;
        }
        
        return sqrDistance - (sphere.Radius + _radius) * (sphere.Radius + _radius) <= 0;
    }

    private void OnDrawGizmos()
    {
        // カプセルの空間との変換行列
        var forward         = Vector3.zero;
        forward[(int)_direction] = 1.0f;
        var capsuleToLocal  = Matrix4x4.TRS(_center, Quaternion.LookRotation(forward), Vector3.one);
        var worldToCapsule  = capsuleToLocal.inverse * transform.worldToLocalMatrix;
        
        // カプセルを構成する二つの半球の中心座標
        var end             = new Vector3(0.0f, 0.0f, 1.0f) * (_height - _radius * 2.0f) / 2.0f;
        var start           = end * -1.0f;

        // Gizmoの変数を変更
        var preColor        = Gizmos.color;
        var preMatrix = Gizmos.matrix;
        Gizmos.color        = Color.blue;
        Gizmos.matrix = worldToCapsule.inverse;
            
        // 球体を描画
        Gizmos.DrawWireSphere(start, _radius);
        Gizmos.DrawWireSphere(end, _radius);
            
        // ラインを描画
        var offsets = new Vector3[]{ new Vector3(-1.0f, 0.0f, 0.0f), new Vector3(0.0f, 1.0f, 0.0f), new Vector3(1.0f, 0.0f, 0.0f), new Vector3(0.0f, -1.0f, 0.0f) };
        for (int i = 0; i < offsets.Length; i++) {
            Gizmos.DrawLine(start + offsets[i] * _radius, end + offsets[i] * _radius);
        }

        // Gizmoの変数を戻す
        Gizmos.matrix       = preMatrix;
        Gizmos.color        = preColor;
    }
}

衝突判定は上記で説明した通りの方法で行っています。

結果

f:id:halya_11:20180815204026g:plain

正常に判定されていることが確認できました。

参考

3D衝突編その1 点と線

関連

light11.hatenadiary.com

light11.hatenadiary.com