2d物理引擎学习 - 解决堆叠不稳定问题

现象

原因的话，还是和之前接触不稳定一样的情况：

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

那之前不都解决过了吗？为啥现在又出现不稳定的情况了？

主要原因是接触点更多了，接触点间的相互影响太大，10次迭代修正根本不够。

那提高迭代次数看看？

100次的时候，倒的没那么快了，所以增加迭代次数是有效果的

增加到500次，倒的更慢了

虽然提高迭代次数有效，但是游戏中不可能这么做，因为会让游戏卡成ppt。

有没有其他的解决办法呢？

那肯定是有的，那怎么做呢？

1) 一帧内不允许这么多次迭代次数，那我改成分帧来迭代是不是就可以了

2) 接触点之间相互影响，主要是它们是独立的，没有关联性，就是自己在修正速度的时候从不去考虑其他接触点会不会有影响；

那是不是可以把碰撞点之间关联起来。

Box2d中类似的解决方法名字叫：连续冲量法（Sequential Impulse） ，是基于约束的求解方式中的一种，

他使用“冲量累加”来解决接触点之间相互影响的问题，使用“热启动”来解决分帧迭代问题（一帧内无法迭代完成，那我保留这次的计算结果，下一帧继续从上次的结果继续迭代）。

基于约束的求解

1) 确定约束方程

比如，之前接触稳定性时，最终确定的约束方程：

2) 将约束方程化简成Ax = b的形式，x即为我们要求的冲量（用冲量修正速度）

其中

b) 的推导：

因为，写成矩阵的表达方式：，可以得到一样的结果

因为ra×n得到的是一个大小值，但这个大小值是z轴的一个分量，所以再×ra，要按3d方式计算：

c) 方程的左侧改成矩阵的写法：

d) 方程的右侧改成矩阵写法：

e) 最终化简得出：(J * M^-1 * J^T) * Δp = -J * V，所以A对应J * M^-1 * J^T，x对应Δp，b对应-J * V

其中

3) 解方程

解方程的方式有很多，但这边我们要找这样一种解方程方式：1) 可以迭代去解，2) 一次性解不出来还能保留计算结果下次继续解。

Projected Jacobi 与 Projected Gauss Seidel解方程法，都可以满足上面的要求，这边的话选用Projected Gauss Seidel法。

以上是从我的理解介绍了下大概的原理，如果对理论感兴趣的参考这边：

2D 游戏物理引擎 - 求解器 - 知乎 (zhihu.com)

2D 游戏物理引擎 - 碰撞约束 - 知乎 (zhihu.com)

游戏物理引擎(四) 约束 - 知乎 (zhihu.com)

效果

代码

public class MyRigidbody : MonoBehaviour
{
    private int m_Id;
    [SerializeField]
    private Vector2 m_Size; //刚体形状现在固定为Box

    [SerializeField]
    private float m_Friction = 0.2f; //表面摩擦系数

    //---------- 线性运动
    [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 * 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];
    public int m_NumContacts;
}

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

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

    public float m_MassNormal; //碰撞法线方向有效质量
    public float m_MassTangent;
}

public class MyPhysics : MonoBehaviour
{
    public int m_MaxIterCount = 10;

    [SerializeField]
    private Vector2 m_Gravity = Vector2.zero;
    [SerializeField]
    private bool m_WarmStarting = false; //碰撞点没变化时, 沿用之前碰撞点的冲量信息
    [SerializeField]
    private bool m_AccumulateImpulses = false; //冲量累加
    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, 如果有一帧没更新, 就说明那一帧没发生碰撞

        int oldNumContacts = collisionInfo.m_NumContacts;
        collisionInfo.m_NumContacts = m_Sat.NumContacts;
        if (collisionInfo.m_Stage == CollisionStage.None) //第1次碰撞
        {
            collisionInfo.m_Stage = CollisionStage.Enter;

            for (int i = 0; i < collisionInfo.m_NumContacts; ++i)
                collisionInfo.m_Contacts[i] = m_Sat.GetContact(i);
        }
        else
        {
            for (int i = 0; i < collisionInfo.m_NumContacts; ++i)
            {
                var contactInfo = m_Sat.GetContact(i);
                if (m_WarmStarting)
                {
                    for (int j = 0; j < oldNumContacts; ++j)
                    {
                        var oldContactInfo = collisionInfo.m_Contacts[j];
                        if (contactInfo.m_Fp == oldContactInfo.m_Fp) //碰撞点没变, 冲量继续沿用
                        {
                            contactInfo.m_ImpulseNormal = oldContactInfo.m_ImpulseNormal;
                            contactInfo.m_ImpulseTangent = oldContactInfo.m_ImpulseTangent;
                        }
                    }
                }
                collisionInfo.m_Contacts[i] = 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)
        {
            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 PreSeperation(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;

            Vector2 normal = contact.m_Normal;
            Vector2 tangent = new Vector2(normal.y, -normal.x); //切线(顺时针)

            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);            
            contact.m_MassNormal = 1 / kMassNormal;

            float kMassTangent = rigidbodyA.InvMass + rigidbodyB.InvMass;
            float raT = Vector2.Dot(ra, tangent);
            float rbT = Vector2.Dot(rb, tangent);
            kMassTangent += rigidbodyA.InvInertia * (Vector2.Dot(ra, ra) - raT * raT) + rigidbodyB.InvInertia * (Vector2.Dot(rb, rb) - rbT * rbT);
            contact.m_MassTangent = 1 / kMassTangent;

            if (m_AccumulateImpulses)
            {
                Vector2 impulse = contact.m_ImpulseNormal * normal; //冲量大小转成冲量向量
                impulse += contact.m_ImpulseTangent * tangent; //切线方向

                rigidbodyA.ApplyImpulse(-impulse);
                rigidbodyA.ApplyTorqueImpulse(ra, -impulse);

                rigidbodyB.ApplyImpulse(impulse);
                rigidbodyB.ApplyTorqueImpulse(rb, impulse);
            }
        }
    }

    private void PostSeperation(float dt, CollisionPair collisionPair)
    {
        var rigidbodyA = collisionPair.m_RigidbodyA;
        var rigidbodyB = collisionPair.m_RigidbodyB;
        float mergeFriction = Mathf.Sqrt(rigidbodyA.Friction * rigidbodyB.Friction);

        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;

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

            if (m_AccumulateImpulses)
            {
                float deltaPNBak = deltaPN;
                float lastImpulseN = contact.m_ImpulseNormal;
                contact.m_ImpulseNormal += deltaPN; //叠加本次冲量(冲量=Δp)
                if (contact.m_ImpulseNormal <= 0) //防止弹开过程中, 变成拉回来的冲量
                {
                    contact.m_ImpulseNormal = 0;
                    deltaPN = -lastImpulseN;
                }
            }
            else
            {
                deltaPN = Mathf.Max(deltaPN, 0); //冲量为负, 碰撞后就加速了, 这样不对
            }

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

            rigidbodyB.ApplyImpulse(impulseN);
            rigidbodyB.ApplyTorqueImpulse(rb, impulseN);

            //摩擦力(切线方向)
            relativeV = rigidbodyB.GetPointVelocity2(rb) - rigidbodyA.GetPointVelocity2(ra); //上面的冲量生效后, 再计算相对速度

            var tangent = new Vector2(normal.y, -normal.x); //切线(顺时针)
            float relativeVT = Vector2.Dot(relativeV, tangent); //投影到切线方向
            relativeVT = -relativeVT; //取反让值为正(与运动方向相同)
            float deltaPT = relativeVT * contact.m_MassTangent;

            if (m_AccumulateImpulses)
            {
                float deltaPTBak = deltaPT;
                float maxDeltaPt = mergeFriction * contact.m_ImpulseNormal; //不能超过法向量的冲量
                float lastImpulseT = contact.m_ImpulseTangent;
                contact.m_ImpulseTangent = Mathf.Clamp(contact.m_ImpulseTangent + deltaPT, -maxDeltaPt, maxDeltaPt);
                deltaPT = contact.m_ImpulseTangent - lastImpulseT;
            }
            else
            {
                float maxDeltaPt = mergeFriction * deltaPN;
                deltaPT = Mathf.Clamp(deltaPT, -maxDeltaPt, maxDeltaPt);
            }

            Vector2 impulseT = deltaPT * tangent;
            rigidbodyA.ApplyImpulse(-impulseT); //摩擦力与运动(趋势)方向相反
            rigidbodyA.ApplyTorqueImpulse(ra, -impulseT);

            rigidbodyB.ApplyImpulse(impulseT);
            rigidbodyB.ApplyTorqueImpulse(rb, impulseT);
        }

    }

    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

}