2d物理引擎学习 - 两Box碰撞后反弹

public class MyRigidbody : MonoBehaviour
{
    private int m_Id;
    [SerializeField]
    private Vector2 m_Size; //刚体形状现在固定为Box//---------- 线性运动
    [SerializeField]
    private float m_Mass; //质量
    private float m_InvMass;

    private Vector2 m_Force; //持续作用的力
    private Vector2 m_ForceImpulse; //脉冲力

    [SerializeField]
    private Vector2 m_Velocity; //当前移动速度
    [SerializeField]
    private Vector2 m_Position; //当前位置
    //----------

    //----------角运动
    private float m_Inertia; //转动惯量
    private float m_InvInertia;

    private float m_Torque; //持续作用的力矩
    private float m_TorqueImpulse; //冲量矩

    [SerializeField]
    private float m_AngleVelocity; //角速度(弧度)
    [SerializeField]
    private float m_Rotation; //旋转角度
    //----------

    void Start()
    {
        if (m_Mass <= 0)
        {
            Mass = float.PositiveInfinity;
            Inertia = float.PositiveInfinity;
        }
        else
        {
            Mass = m_Mass;
            Inertia = m_Mass * (m_Size.x * m_Size.x + m_Size.y * m_Size.y) / 12.0f;
        }
    }

    public MyRigidbody() { }

    public MyRigidbody(float mass, float inertia)
    {
        Mass = mass;
        Inertia = inertia;
    }

    public int Id
    {
        get { return m_Id; }
        set { m_Id = value; }
    }

    public Vector2 Size
    {
        get { return m_Size; }
        set { m_Size = value; }
    }

    public float Friction
    {
        get { return m_Friction; }
        set { m_Friction = value; }
    }

    //---------- 线性运动
    public float Mass
    {
        get { return m_Mass; }
        set
        {
            m_Mass = value;
            if (value >= float.PositiveInfinity)
                m_InvMass = 0;
            else
                m_InvMass = 1 / value;
        }
    }

    public float InvMass
    {
        get { return m_InvMass; }
    }

    public Vector2 Position
    {
        get { return m_Position; }
        set { m_Position = value; }
    }

    public Vector2 Velocity
    {
        get { return m_Velocity; }
    }

    //线性冲量产生线速度变化
    public void ApplyImpulse(Vector2 impulse)
    {
        m_Velocity += impulse * m_InvMass; // 动量定理: I = Δp = m * Δv
    }

    //----------

    //----------角运动

    public float Inertia
    {
        get { return m_Inertia; }
        set
        {
            m_Inertia = value;
            if (value >= float.PositiveInfinity)
                m_InvInertia = 0;
            else
                m_InvInertia = 1 / value;
        }
    }

    public float InvInertia
    {
        get { return m_InvInertia; }
    }

    public float AngleVelocity
    {
        get { return m_AngleVelocity; }
    }

    public float Rotation
    {
        get { return m_Rotation; }
        set { m_Rotation = value; }
    }

    /// <summary>
    /// 线性冲量产生角速度变化
    /// </summary>
    /// <param name="point">力作用点</param>
    /// <param name="impulse">线性冲量</param>
    public void ApplyTorqueImpulse(Vector2 point, Vector2 impulse)
    {
        Vector2 r = point - m_Position;
        //角冲量: H = r × 线性冲量; r为力臂向量(或叫矢径), r与线性冲量方向垂直;
        float torqueImpulse = r.x * impulse.y - r.y * impulse.x;
        //角动量定理: H = ΔL = I * Δω
        //所以角速度的变化: Δω = H / I
        m_AngleVelocity += torqueImpulse * m_InvInertia;
    }

    //----------

    public float GetKMass(Vector2 point, Vector2 normal)
    {
        float kMass = m_InvMass;
        
        Vector2 rVec = point - m_Position;
        float rn = Vector2.Dot(rVec, normal);
        float rSqr = rVec.sqrMagnitude - rn * rn;

        //惯量: I = m * r^2, r为与线性冲量方向垂直的力臂
        // >>> 1/m = r^2 / I
        kMass += rSqr * m_InvInertia;
        
        return kMass;
    }

    public Vector2 GetPointVelocity(Vector2 point)
    {
        Vector2 r = point - m_Position;
        //角运动线速度: v = r * dir * ω, dir为线速度方向
        Vector2 v = new Vector2(-r.y, r.x) * m_AngleVelocity;
        return m_Velocity + v;
    }

    //计算力和冲量引起的速度变化
    public void PreUpdate(Vector2 gravity, float dt)
    {
        //----- 持续力
        //a = F / m
        //v1 = v0 + a * t
        m_Velocity += (m_Force * m_InvMass + gravity) * dt;
        //角加速度 = 力矩 / 惯量
        //ω1 = ω0 + 角加速度 * t
        m_AngleVelocity += m_Torque * m_InvInertia * dt;
        //-----

        //----- 脉冲力(冲量)
        //动量定理: 冲量 = Δp = m * Δv
        // >>> Δv = 冲量 / m
        m_Velocity += m_ForceImpulse * m_InvMass;
        //角动量定理: 角冲量 = ΔL = 惯量 * Δω
        // >>> Δω = 角冲量 / 惯量
        m_AngleVelocity += m_TorqueImpulse * m_InvInertia;

        m_ForceImpulse = Vector2.zero; //冲量是瞬时效果, 作用完就置零
        m_TorqueImpulse = 0;
        //-----
    }

    //根据速度进行运动
    public void PostUpdate(float dt)
    {
        m_Position += m_Velocity * dt;
        m_Rotation += m_AngleVelocity * dt;
    }


#if UNITY_EDITOR
    public Color m_GizmosColor = Color.white;

    private Vector2[] m_TempVerts = new Vector2[4];

    private void UpdateCorners()
    {
        Vector2 halfSize = m_Size * 0.5f;
        m_TempVerts[0] = -halfSize; //lb
        m_TempVerts[1] = new Vector2(-halfSize.x, halfSize.y); //lt
        m_TempVerts[2] = halfSize; //rt
        m_TempVerts[3] = new Vector2(halfSize.x, -halfSize.y); //rb
    }

    public Vector2[] GetVerts()
    {
        UpdateCorners();
        for (int i = 0; i < m_TempVerts.Length; ++i)
            m_TempVerts[i] = this.transform.TransformPoint(m_TempVerts[i]);

        return m_TempVerts;
    }

    private void OnDrawGizmos()
    {
        if (m_Size.sqrMagnitude <= 0)
            return;

        var oldColor = Gizmos.color;
        Gizmos.color = m_GizmosColor;

        var verts = GetVerts();

        var trans = this.transform;
        if (Application.isPlaying)
        {
            trans.position = m_Position;
            trans.eulerAngles = new Vector3(0, 0, Mathf.Rad2Deg * m_Rotation);
        }
        else
        {
            m_Position = trans.position;
            m_Rotation = trans.eulerAngles.z * Mathf.Deg2Rad;
        }

        for (int i = 0; i < verts.Length; ++i)
        {
            var pos1 = verts[i];
            var pos2 = verts[(i + 1) % verts.Length];
            Gizmos.DrawLine(pos1, pos2);
        }

        Gizmos.color = oldColor;
    }

#endif

}

enum CollisionStage
{
    None,
    Enter,
    Stay,
    Exit,
}

//CollisionPair使用两个刚体的id作为索引
struct CollisionPairKey
{
    public int m_IdA;
    public int m_IdB;

    public CollisionPairKey(int idA, int idB)
    {
        m_IdA = idA;
        m_IdB = idB;
    }
}

class CollisionPair
{
    public int m_UpdateIndex; //发生碰撞时的帧
    public MyRigidbody m_RigidbodyA;
    public MyRigidbody m_RigidbodyB;

    public CollisionStage m_Stage = CollisionStage.None;

    public ContactInfo[] m_Contacts = new ContactInfo[2]; //box最多2个碰撞点
    public int m_NumContacts;
}

//碰撞点信息
public class ContactInfo
{
    public Vector2 m_Point; //碰撞点
    public Vector2 m_Normal; //碰撞法向量(分离方向), 这边用A指向B, 即: B反弹方向
    public float m_Penetration; //穿透深度(分离距离)
}

public class MyPhysics : MonoBehaviour
{
    [Range(0, 1)]
    public float m_Elasticity = 0;
    private List<MyRigidbody> m_RigidbodyList = new List<MyRigidbody>();

    private List<MyRigidbody> m_PendingAddList = new List<MyRigidbody>(); //要添加的刚体会在下一帧添加
    private List<MyRigidbody> m_PendingRemoveList = new List<MyRigidbody>(); //要删除的刚体在下一帧删除

    private Dictionary<CollisionPairKey, CollisionPair> m_CollisionPairDict = new Dictionary<CollisionPairKey, CollisionPair>(); //两个发生碰撞的物体
    private List<CollisionPairKey> m_TempRemoveCollisionPairList = new List<CollisionPairKey>();

    private int m_IdCounter; //刚体id计数
    private int m_UpdateCounter; //更新计数

    private B2SatCollide m_Sat = new B2SatCollide();

    void Start()
    {
        var initRigidbodys = GetComponentsInChildren<MyRigidbody>();
        foreach (var rigidbody in initRigidbodys)
        {
            AddRigidbody(rigidbody);
        }
    }

    void FixedUpdate()
    {
        if (Time.fixedDeltaTime > 0)
            Step(Time.fixedDeltaTime);
    }

    public void Step(float dt)
    {
        CheckPendingList();
        m_UpdateCounter++;

        for (int i = 0; i < m_RigidbodyList.Count; ++i)
        {
            var rigidbody = m_RigidbodyList[i];
            if (0 == rigidbody.InvMass)
                continue;
            rigidbody.PreUpdate(m_Gravity, dt);
        }
        CheckCollision();
        UpdateSeperation(dt);

        for (int i = 0; i < m_RigidbodyList.Count; ++i)
        {
            var rigidbody = m_RigidbodyList[i];
            rigidbody.PostUpdate(dt);
        }
    }

    //检查发生碰撞的物体
    private void CheckCollision()
    {
        for (int i = 0; i < m_RigidbodyList.Count; ++i)
        {
            var rigidbodyA = m_RigidbodyList[i];
            for (int j = i + 1; j < m_RigidbodyList.Count; ++j)
            {
                var rigidbodyB = m_RigidbodyList[j];
                if (0 == rigidbodyA.InvMass && 0 == rigidbodyB.InvMass)
                    continue;

                if (m_Sat.Collide(rigidbodyA, rigidbodyB) > 0)
                {
                    if (rigidbodyA.Id < rigidbodyB.Id)
                        OnCollide(rigidbodyA, rigidbodyB);
                    else
                        OnCollide(rigidbodyB, rigidbodyA);
                }
            }
        }
    }

    //碰撞处理
    private void OnCollide(MyRigidbody rigidbodyA, MyRigidbody rigidbodyB)
    {
        var key = new CollisionPairKey(rigidbodyA.Id, rigidbodyB.Id);
        if (!m_CollisionPairDict.TryGetValue(key, out var collisionInfo)) //之前没发生过碰撞(第1次碰撞)
        {
            collisionInfo = new CollisionPair();
            collisionInfo.m_RigidbodyA = rigidbodyA;
            collisionInfo.m_RigidbodyB = rigidbodyB;
            m_CollisionPairDict.Add(key, collisionInfo);
        }
        collisionInfo.m_UpdateIndex = m_UpdateCounter; //发生了碰撞就更新帧id, 如果有一帧没更新, 就说明那一帧没发生碰撞

        collisionInfo.m_NumContacts = m_Sat.NumContacts;
        for (int i = 0; i < collisionInfo.m_NumContacts; ++i)
            collisionInfo.m_Contacts[i] = m_Sat.GetContact(i);

        if (collisionInfo.m_Stage == CollisionStage.None) //第1次碰撞
        {
            collisionInfo.m_Stage = CollisionStage.Enter;
        }
    }

    //物体发生弹性碰撞, 会相互弹开
    private void UpdateSeperation(float dt)
    {
        foreach (var entry in m_CollisionPairDict)
        {
            var collisionPair = entry.Value;
            if (collisionPair.m_UpdateIndex != m_UpdateCounter) //上一帧没发生碰撞
            {
                collisionPair.m_Stage = CollisionStage.Exit;
            }

            switch (collisionPair.m_Stage)
            {
            case CollisionStage.Enter:
                //todo: 通知Enter事件
                collisionPair.m_Stage = CollisionStage.Stay;
                break;
            case CollisionStage.Exit:
                //todo: 通知Exit事件
                collisionPair.m_Stage = CollisionStage.None;
                var key = new CollisionPairKey(collisionPair.m_RigidbodyA.Id, collisionPair.m_RigidbodyB.Id);
                m_TempRemoveCollisionPairList.Add(key); //for循环中删除会报错
                break;
            }

            if (CollisionStage.Stay == collisionPair.m_Stage)
            {
                //todo: 通知Stay事件
            }
        }

        if (m_TempRemoveCollisionPairList.Count > 0)
        {
            for (int i = 0; i < m_TempRemoveCollisionPairList.Count; ++i)
            {
                var key = m_TempRemoveCollisionPairList[i];
                m_CollisionPairDict.Remove(key);
            }
            m_TempRemoveCollisionPairList.Clear();
        }

        foreach (var entry in m_CollisionPairDict)
        {
            PostSeperation(dt, entry.Value);
        }
    }

    private void PostSeperation(float dt, CollisionPair collisionPair)
    {
        var rigidbodyA = collisionPair.m_RigidbodyA;
        var rigidbodyB = collisionPair.m_RigidbodyB;

        float kMass = rigidbodyA.GetKMass(contact.m_Point, normal) + rigidbodyB.GetKMass(contact.m_Point, normal);
        float massNormal = 1 / kMass;

        for (int i = 0; i < collisionPair.m_NumContacts; ++i)
        {
            var contact = collisionPair.m_Contacts[i];
            var relativeV = rigidbodyB.GetPointVelocity(contact.m_Point) - rigidbodyA.GetPointVelocity(contact.m_Point);

            var normal = contact.m_Normal;
            float relativeVN = Vector2.Dot(relativeV, normal); //投影到法向量
            //if (relativeVN > 0) //相对速度>0时, 表明没有碰撞趋势了
            //    return;

            //Δp = (1 + e) * (v2 - v1) / kMass
            //kMass = 1/m1 + 1/m2
            float deltaPN = (1 + m_Elasticity) * relativeVN * massNormal;
            deltaPN = -deltaPN; //对Δp取反, 主要是为了让累加冲量是正值
            deltaPN = Mathf.Max(deltaPN, 0); //冲量为负, 碰撞后就加速了, 这样不对

            Vector2 impulseN = deltaPN * normal; //转为矢量
            rigidbodyA.ApplyImpulse(-impulseN);
            rigidbodyA.ApplyTorqueImpulse(contact.m_Point, -impulseN);

            rigidbodyB.ApplyImpulse(impulseN);
            rigidbodyB.ApplyTorqueImpulse(contact.m_Point, impulseN);
        }

    }

    private void CheckPendingList()
    {
        if (m_PendingAddList.Count > 0)
        {
            for (int i = 0; i < m_PendingAddList.Count; ++i)
            {
                var rigidbody = m_PendingAddList[i];
                rigidbody.Id = m_IdCounter++;
                rigidbody.OnAddToPhysics();
                m_RigidbodyList.Add(rigidbody);
            }
            m_PendingAddList.Clear();
        }

        if (m_PendingRemoveList.Count > 0)
        {
            for (int i = 0; i < m_PendingRemoveList.Count; ++i)
            {
                var rigidbody = m_PendingRemoveList[i];
                m_RigidbodyList.Remove(rigidbody);
                rigidbody.OnRemoveFromPhysics();
            }
            m_PendingRemoveList.Clear();
        }
    }

    public void AddRigidbody(MyRigidbody rigidbody)
    {
        m_PendingAddList.Add(rigidbody);
    }

    public void RemoveRigidbody(MyRigidbody rigidbody)
    {
        m_PendingRemoveList.Add(rigidbody);
    }


#if UNITY_EDITOR
    public Color m_GizmosColor = Color.white;

    private void OnDrawGizmos()
    {
        var oldColor = Gizmos.color;
        Gizmos.color = m_GizmosColor;
        Handles.color = m_GizmosColor;

        foreach (var entry in m_CollisionPairDict)
        {
            var collisionPair = entry.Value;
            for (int i = 0; i < collisionPair.m_NumContacts; ++i)
            {
                var contactInfo = collisionPair.m_Contacts[i];
                DrawGizmosHelper.DrawPoint2(contactInfo.m_Point);
                DrawGizmosHelper.DrawArrowLine(contactInfo.m_Point, contactInfo.m_Normal, 0.5f);
            }
        }


        Gizmos.color = oldColor;
        Handles.color = oldColor;
    }

#endif

}