碰撞响应

limingyin18
341 阅读2分钟

碰撞响应

两刚体发生碰撞后，基于守恒律解决碰撞后的状态。

算法

碰撞点

刚体AA和刚体BB在xx点处发生碰撞，有：

xa=x−xAxb=x−xB x_a = x - x_A \\ x_b = x - x_B

xAx_A、xBx_B为刚体的位置，xax_a、xbx_b为碰撞点xx在刚体的局部坐标。

va=vA+ωA×xavb=vB+ωB×xb v_a = v_A + \omega_A \times x_a \\ v_b = v_B + \omega_B \times x_b

vAv_A、vBv_B为刚体的速度，ωA\omega_A、ωB\omega_B为刚体的角速度，vav_a、vbv_b为碰撞点的速度。

vrel=va−vbvn=(n⋅vrel)nvt=vrel−vn \begin{aligned} v_{rel} &= v_a - v_b \\ v_n &= (n \cdot v_{rel})n \\ v_t &= v_{rel} - v_n \end{aligned}

vrelv_{rel} 为碰撞相对速度，vnv_n为碰撞法向速度，vtv_t为碰撞切线速度。

vn+=−cor⋅vn v_n^+ = -cor \cdot v_n

vn+v_n^+为碰撞后的法向速度，corcor为恢复系数，1为完全弹性碰撞，0为完全粘性碰撞。

a=max(1−cof(1+cor)∗∣∣vn∣∣/∣∣vt∣∣,0)vt+=a⋅vt a = max(1-cof(1+cor)*||v_n||/||v_t||, 0) \\ v_t^+ = a \cdot v_t

vt+v_t^+为碰撞后的切线速度，cofcof为摩擦系数，0为无摩擦碰撞，摩擦最大为把切线速度减至0。

Δv=vn++vt+−vrel \Delta v = v_n^++v_t^+ - v_{rel}

Δv\Delta v为碰撞点处速度的改变量

摩擦

根据Coulomb friction，摩擦力的大小与正压力的大小成正比：

fc=cof⋅fnfcΔt=cof⋅fnΔtvt+−vt−=cof(vn+−vn−)avt−−vt−=cof(−cor⋅vn−−vn−)avt−−vt−=cof(−cor⋅vn−−vn−)(a−1)vt−=−cof(cor+1)vn−a=1−cof(1+cor)∣∣vn−∣∣/∣∣vt−∣∣ \begin{aligned} f_c &=cof \cdot f_n \\ f_c \Delta t &=cof \cdot f_n \Delta t \\ v_t^+-v_t^-&=cof(v_n^+-v_n^-) \\ av_t^--v_t^-&=cof(-cor\cdot v_n^--v_n^-) \\ av_t^--v_t^-&=cof(-cor\cdot v_n^--v_n^-) \\ (a-1)v_t^- &= -cof(cor+1)v_n^- \\ a &= 1-cof(1+cor)||v_n^-||/||v_t^-|| \end{aligned}

刚体冲量

为了在碰撞点处产生Δv\Delta v的改变量，需要在碰撞点处施加冲量JJ。

假设在碰撞点处施加冲量JJ，则刚体的状态变化有：

v+=v+m−1Jω+=ω+I−1(x×J) \begin{aligned} \bm{v}^+ &= \bm{v} + m^{-1}J \\ \bm{\omega}^+ &= \bm{\omega} + I^{-1}(x \times J) \end{aligned}

碰撞点处的速度变化：

v+=v++ω+×xv+=v+m−1J+(ω+I−1(x×J))×xv+=v+ω×x+m−1J+(I−1(x×J))×xv+=v−+m−1J+(I−1(x×J))×xv+−v−=m−1J+(I−1(x×J))×xΔv=m−1J+(I−1(x×J))×xΔv=m−1J−x×(I−1(x×J)) \begin{aligned} v^+ &= \bm{v}^++\bm{\omega}^+ \times x \\ v^+ &= \bm{v} + m^{-1}J + (\bm{\omega} + I^{-1}(x \times J)) \times x \\ v^+ &= \bm{v} + \bm{\omega}\times x + m^{-1}J + (I^{-1}(x \times J)) \times x \\ v^+ &= v^- + m^{-1}J + (I^{-1}(x \times J)) \times x \\ v^+ - v^- &= m^{-1}J + (I^{-1}(x \times J)) \times x \\ \Delta v &= m^{-1}J + (I^{-1}(x \times J)) \times x \\ \Delta v &= m^{-1}J - x \times (I^{-1}(x \times J)) \\ \end{aligned}

用负的斜对称矩阵来代替叉乘：

Δv=m−1J−x×(I−1(x×J))Δv=m−1J−(x)∗(I−1((x)∗J))Δv=[m−1−(x)∗(I−1((x)∗))]JΔv=KJ \begin{aligned} \Delta v &= m^{-1}J - x \times (I^{-1}(x \times J)) \\ \Delta v &= m^{-1}J - (x)^*(I^{-1}((x)^*J)) \\ \Delta v &= [m^{-1} - (x)^*(I^{-1}((x)^*))]J \\ \Delta v &= KJ \\ \end{aligned}

两个刚体碰撞，由守恒律知施加在刚体a上的冲量与b上的大小相等，方向相反：

Ja=−Jb J_a=-J_b

所以：

Δv=Δva−ΔvbΔv=[ma−1−(xa)∗(I−1((xa)∗))]Ja−[mb−1−(xb)∗(I−1((xb)∗))]JbΔv=[ma−1+mb−1−((xa)∗(I−1((xa)∗))+(xb)∗(I−1((xb)∗)))]JΔv=KJ \begin{aligned} \Delta v &= \Delta v_a - \Delta v_b \\ \Delta v &= [m_a^{-1} - (x_a)^*(I^{-1}((x_a)^*))]J_a - [m_b^{-1} - (x_b)^*(I^{-1}((x_b)^*))]J_b \\ \Delta v &= [m_a^{-1} + m_b^{-1} - ((x_a)^*(I^{-1}((x_a)^*)) + (x_b)^*(I^{-1}((x_b)^*)))]J \\ \Delta v &= KJ \\ \end{aligned}

程序


void ForceBasedDynamics::collisionResponseByImpulse(CranePhysics::Rigidbody &rba, CranePhysics::Rigidbody &rbb,
                                                    Eigen::Vector3f n, Eigen::Vector3f x, float cor, float cof)
{
    n = -n;
    Vector3f xa = x - rba.GetPosition();
    Vector3f xb = x - rbb.GetPosition();
    Vector3f va = rba.GetVelocity() + rba.GetAngularVelocity().cross(xa);
    Vector3f vb = rbb.GetVelocity() + rbb.GetAngularVelocity().cross(xb);
    Vector3f v_rel = va - vb;
    float v_rel_f = n.dot(va - vb);
    Vector3f v_n_rel = v_rel_f * n;

    if (v_rel_f > 0.f)
    {
        return;
    }

    Vector3f v_n_plus = -cor * v_n_rel;

    Vector3f v_t_minus = v_rel - v_n_rel;
    Vector3f t = v_t_minus.normalized();
    float a = std::max(1.f - cof * (1.f + cor) * v_n_rel.norm() / v_t_minus.norm(), 0.f);
    Vector3f v_t_plus = a * v_t_minus;

    Vector3f delta_v = v_n_plus + v_t_plus - v_rel;

    Matrix3f K = rba.GetMassInv() * Matrix3f::Identity() + rbb.GetMassInv() * Matrix3f::Identity() -
                 ((-1.f * (xa).asSkewSymmetric()) * rba.GetInertiaMomentInv() * (-1.f * (xa).asSkewSymmetric()) +
                  (-1.f * (xb).asSkewSymmetric()) * rbb.GetInertiaMomentInv() * (-1.f * (xb).asSkewSymmetric()));
    Vector3f J = K.inverse() * delta_v;

    rba.SetVelocity(rba.GetVelocity() + rba.GetMassInv() * J);
    rba.SetAngularVelocity(rba.GetAngularVelocity() + rba.GetInertiaMomentInv() * (xa.cross(J)));

    rbb.SetVelocity(rbb.GetVelocity() + rbb.GetMassInv() * -J);
    rbb.SetAngularVelocity(rbb.GetAngularVelocity() + rbb.GetInertiaMomentInv() * (xb.cross(-J)));

    /*
    if (v_t_minus.norm() > 0.F)
    {
        {
            float fn_a = (-rba.force_ * rba.GetMassInv()).dot(-n);
            float a1 = std::max(1.f - cof * fn_a * dt / v_t_minus.norm(), 0.f);
            float a2 = std::min(a1, 1.f);
            Vector3f va = rba.GetVelocity();
            Vector3f vna = va.dot(n) * n;
            Vector3f vta = va - vna;
            Vector3f vta_plus = vta * a2;
            Vector3f va_plus = vta_plus + vna;
            rba.SetVelocity(va_plus);
        }

        {
            float fn_b = (-rbb.force_ * rbb.GetMassInv()).dot(n);
            float a1 = std::max(1.f - cof * fn_b * dt / v_t_minus.norm(), 0.f);
            float a2 = std::min(a1, 1.f);
            Vector3f vb = rbb.GetVelocity();
            Vector3f vnb = vb.dot(n) * n;
            Vector3f vtb = vb - vnb;
            Vector3f vtb_plus = vtb * a2;
            Vector3f vb_plus = vtb_plus + vnb;
            rbb.SetVelocity(vb_plus);
        }
    }
    */
}

测试

ezgif.com-video-to-gif.gif

参考

  1. 王华民-GAMES103-基于物理的计算机动画入门
avatar
文章
阅读
粉丝