2d物理引擎学习 - 两圆的线性运动碰撞反馈

public class MyRigidbody : MonoBehaviour
{
    private int m_Id;
    [SerializeField]
    private float m_Radius; //精简了形状, 用圆半径代替

    //---------- 线性运动
    [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; //当前位置
    //----------

    void Start()
    {
        if (m_Mass < 0)
            m_Mass = float.PositiveInfinity;
        Mass = m_Mass;
    }

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

    public float Radius
    {
        get { return m_Radius; }
        set { m_Radius = 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 Velocity
    {
        get { return m_Velocity; }
    }

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

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

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

    //计算力和冲量引起的速度变化
    public void PreUpdate(float dt)
    {
        //----- 持续力
        //a = F / m
        //v1 = v0 + a * t
        m_Velocity += m_Force * m_InvMass * dt;
        //-----

        //----- 脉冲力(冲量)
        //动量定理: 冲量 = Δp = m * Δv
        // >>> Δv = 冲量 / m
        m_Velocity += m_ForceImpulse * m_InvMass;

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

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


#if UNITY_EDITOR
    public bool m_ShowRadius;

    private void OnDrawGizmos()
    {
        if (m_Radius <= 0)
            return;

        var trans = this.transform;
        if (Application.isPlaying)
            trans.position = Position;
        else
            Position = trans.position;

        GizmosDrawHelper.DrawCircle(trans.position, m_Radius);
        if (m_ShowRadius)
            Gizmos.DrawLine(trans.position, trans.TransformPoint(Vector3.right * m_Radius));
    }

#endif

}

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

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

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

public 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[1]; //圆只有一个碰撞点
}


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

    public float m_ImpulseNormal; //法线方向累加冲量

    public float m_MassNormal; //碰撞后速度计算公式中会用到: 1/m1+1/m2, 这里存放的就是这个结果
}

public class MyPhysics : MonoBehaviour
{
    public MyRigidbody[] m_InitRigidbodys;

    private int m_MaxIterCount = 10;

    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; //更新计数 

    void Start()
    {
        //在编辑器Inspector上设置的刚体
        foreach (var rigidbody in m_InitRigidbodys)
        {
            if (null != rigidbody)
                AddRigidbody(rigidbody);
        }
    }

    void Update()
    {
        Step(Time.deltaTime);
    }

    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(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 (Shape2DHelper.IsTwoCircleIntersect(rigidbodyA.Position, rigidbodyA.Radius, rigidbodyB.Position, rigidbodyB.Radius)) //精检测
                {
                    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, 如果有一帧没更新, 就说明那一帧没发生碰撞

        //本次碰撞的碰撞点信息
        var contactInfo = new ContactInfo();
        float totalR = (rigidbodyA.Radius + rigidbodyB.Radius);
        var circleDistVec = rigidbodyB.Position - rigidbodyA.Position;
        contactInfo.m_Normal = circleDistVec.normalized; //碰撞法线为圆心连线方向
        contactInfo.m_Penetration = totalR - circleDistVec.magnitude; //穿透深度
        contactInfo.m_Point = rigidbodyA.Position + (rigidbodyA.Radius - contactInfo.m_Penetration * 0.5f) * contactInfo.m_Normal; //碰撞点在穿透向量中点处

        if (collisionInfo.m_Stage == CollisionStage.None) //第1次碰撞
        {
            collisionInfo.m_Stage = CollisionStage.Enter;
            collisionInfo.m_Contacts[0] = contactInfo;
        }
        else
        {
            //检查碰撞点是否发生变化
            foreach (var oldContactInfo in collisionInfo.m_Contacts)
            {
                if ((oldContactInfo.m_Point - contactInfo.m_Point).sqrMagnitude <= float.Epsilon) //碰撞点没变, 冲量继续沿用
                {
                    contactInfo.m_ImpulseNormal = oldContactInfo.m_ImpulseNormal;
                }
            }
            collisionInfo.m_Contacts[0] = contactInfo;
        }
    }

    //物体发生弹性碰撞, 会相互弹开
    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事件
                PreSeperation(dt, collisionPair);
            }
        }

        if (m_TempRemoveCollisionPairList.Count > 0)
        {
            foreach (var key in m_TempRemoveCollisionPairList)
            {
                m_CollisionPairDict.Remove(key);
            }
            m_TempRemoveCollisionPairList.Clear();
        }

        for (int i = 0; i < m_MaxIterCount; ++i)
        {
            foreach (var entry in m_CollisionPairDict)
            {
                PostSeperation(dt, entry.Value);
            }
        }
    }

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

        foreach (var contact in collisionPair.m_Contacts)
        {
            Vector2 normal = contact.m_Normal;

            float kMassNormal = rigidbodyA.InvMass + rigidbodyB.InvMass;
            contact.m_MassNormal = 1 / kMassNormal;

            Vector2 impulse = contact.m_ImpulseNormal * normal; //冲量大小转成冲量向量
            rigidbodyA.ApplyImpulse(-impulse);
            rigidbodyB.ApplyImpulse(impulse);
        }
    }

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

        foreach (var contact in collisionPair.m_Contacts)
        {
            var relativeV = rigidbodyB.Velocity - rigidbodyA.Velocity;

            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 e = 1;
            float deltaPN = (1 + e) * relativeVN * contact.m_MassNormal;
            deltaPN = -deltaPN; //对Δp取反, 主要是为了让累加冲量是正值
            float lastImpulseN = contact.m_ImpulseNormal;
            contact.m_ImpulseNormal += deltaPN; //叠加本次冲量(冲量=Δp)
            if (contact.m_ImpulseNormal <= 0) //防止弹开过程中, 变成拉回来的冲量
            {
                contact.m_ImpulseNormal = 0;
                deltaPN = -lastImpulseN;
            }
 
            Vector2 impulse = deltaPN * normal; //转为矢量
            rigidbodyA.ApplyImpulse(-impulse);
            rigidbodyB.ApplyImpulse(impulse);
        }
    }

    //检查要添加和删除的刚体
    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++;
                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);
            }
            m_PendingRemoveList.Clear();
        }
    }

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

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

}