2d物理引擎学习 - 斜坡上下滑的物体

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;

        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 massEffectiveT = 1 / kMassNormal;
        //Δp = (1 + e) * (v2 - v1) / kMass
        float deltaPN = (1 + m_Elasticity) * relativeVN * massEffectiveT;
        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);

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

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

        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);
        float massEffectiveT = 1 / kMassTangent;
        float deltaPT = relativeVT * massEffectiveT;
        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);
    }

}