2d物理引擎学习 - 基于约束的公式解决接触稳定性问题

public static Vector2 ZCross(float z, Vector2 v2)
{
    Vector2 result = new Vector2(-z * v2.y, z * v2.x);
    return result;
}

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)
    {
        // 动量定理: I = Δp = m * Δv
        m_Velocity += impulse * m_InvMass; 
    }

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

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

    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="r">矢径, 物体质心指向碰撞点</param>
    /// <param name="impulse">线性冲量</param>
    public void ApplyTorqueImpulse(Vector2 r, Vector2 impulse)
    {
        //角冲量: 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 Vector2 GetPointVelocity(Vector2 r)
    {
        //角运动线速度: v = ω × r, r是矢径;
        //或v = ω * r * dir, 此处的r是力臂长度, 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_Radius > 0)
        {
            var oldColor = Gizmos.color;
            Gizmos.color = m_GizmosColor;

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

            DrawGizmosHelper.DrawCircle(trans.position, m_Radius);
            Gizmos.DrawLine(trans.position, trans.TransformPoint(Vector3.right * m_Radius));

            Gizmos.color = oldColor;
            return;
        }

        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
{
    public int m_MaxIterCount = 10;

    [SerializeField]
    private Vector2 m_Gravity = Vector2.zero;
    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)
            return;
         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();
        }

        for (int i = 0; i < m_MaxIterCount; ++i)
        {
            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;

        for (int i = 0; i < collisionPair.m_NumContacts; ++i)
        {
            var contact = collisionPair.m_Contacts[i];
            Vector2 ra = contact.m_Point - rigidbodyA.Position;
            Vector2 rb = contact.m_Point - rigidbodyB.Position;

            var relativeV = rigidbodyB.GetPointVelocity(rb) - rigidbodyA.GetPointVelocity(ra);

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

            float kMassNormal = rigidbodyA.InvMass + rigidbodyB.InvMass;
            float raN = Vector2.Dot(ra, normal);
            float rbN = Vector2.Dot(rb, normal);
            kMassNormal += rigidbodyA.InvInertia * (Vector2.Dot(ra, ra) - raN * raN) + rigidbodyB.InvInertia * (Vector2.Dot(rb, rb) - rbN * rbN);
            float massEffective = 1 / kMassNormal;

            //Δp = (v2 - v1) / kMass
            float deltaPN = relativeVN * massEffective;
            deltaPN = -deltaPN; //对Δp取反, 主要是为了让累加冲量是正值
            deltaPN = Mathf.Max(deltaPN, 0);

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

            rigidbodyB.ApplyImpulse(impulseN);
            rigidbodyB.ApplyTorqueImpulse(rb, 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

}