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

先看下直接用弹性碰撞的公式，会出现的问题：

Box落在地面上后，没有停在地面上，而是还在不断的下沉。

弹性碰撞公式处理碰撞后弹开没有大问题，但是处理物体碰撞后的接触存在不稳定问题。

如何解决？

目前物理引擎最主流的解决方法是：基于约束来组织物理公式，而不是直接套用物理公式。

什么叫约束？就是让两个物体满足我们设定的规则（专业点叫约束方程）。

接触过程中涉及的约束

下图中下方的物体为A，上方的物体为B

1) 要让B不再下沉，就是要让两者维持现有穿透距离不再变化

穿透距离的函数可以这么表示：C = (a-b) • n，C是constraint的缩写，n是碰撞法线方向

2) 怎么让该函数不再变化？

函数的导数为0时，函数值就不再变化。

导数tips：导数表示的是函数的变化率，一元函数的导数为变量前的系数。

因为 x = v * t，即：距离 = 速度 * 时间，而cd其实就是一段距离，它也是和时间有关，在随着时间慢慢变大。

函数写成关于时间t的表示方法：C = (v_pa*t - v_pb*t) • n，v_pa表示点a处的合速度_{，v_pb}表示点b处的合速度

因为接触法线方向不经过物体的质心，所以碰撞后会导致物体旋转，所以这边的速度是线速度和角速度的合速度

v_pa = va + ωa × ra

v_pb = vb + ωb × rb

ra = a - Oa

rb = b - Ob

3) 求函数C的导数C'

因为现在要让导数为0，即C' = (v_pa - v_pb) • n = 0，所以要将速度进行修正，修正到碰撞点的相对速度在碰撞法线上的投影值为0。

改变速度的方式有很多，这边选择物理引擎中常用的方法，用冲量的来改变速度（至于为啥是冲量，没去深纠，暂时认为是一种最佳实践吧）

4) 下面其实就和两物体碰撞后反弹的公式差不多了

冲量 = 动量的变化：n表示碰撞法向量

所以，施加冲量后的线速度和角速度

碰撞点处的合线速度

施加冲量修正速度后的相对速度

因为要求修正后碰撞点的相对速度在碰撞法线上的投影值为0，所以

方程右侧的分母部分，也叫作有效质量

根据拉格朗日公式（二重向量叉乘化简公式）

化简后

一顿操作猛如虎，这个方程和弹性碰撞后速度的方程没有区别，只要把弹性系数e=0，就完全一样。那不还是没法解决接触稳定性问题嘛！！！！！！

是的，但是从约束的角度考虑物理公式，就给了我们进一步修正的余地；直接套用物理公式，出问题时除了改物理公式没其他余地。

5) 分析原因

5-a) 浮点数误差，冲量修正后的速度存在轻微误差，但随着时间不断叠加，就慢慢的变成大误差了，表现就是慢慢下沉。

5-b) 物理公式中的接触面是连续的整体，而代码中是用的2个离散的接触点，且还是独立的，没有关联性；那有没有可能出现，对A点用冲量修正后对了，再对B点用冲量修正后，B点对了但A点又不对了。

解决办法：A, B两点修正完后，我再修正一次，甚至迭代修正多次，让修正后的偏差变小。

这个方法可行，但并没有完全消除掉修正后的偏差。不过在这边不做处理了，因为已经看不出来了；后面讲到堆叠问题再处理，因为相关理论会更复杂点

6) 看下效果

迭代修正3次后，下沉变慢了

迭代修正10次，偏差值更小了

迭代修正50次，看上去没偏差了。虽然50次很好，但真正在游戏中的话，一帧不可能让迭代50次的，会卡成ppt的。

下面的公式合法吗？

v_pa = va + ωa × ra，因为ωa是一个标量，标量能叉乘矢量ra？

先看张图

ωa在这边其实表示的是一个z值，要转成3d向量来看

工具函数

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_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

}