碰撞解决（无摩擦力）

limingyin18
339 阅读1分钟

介绍

计算碰撞冲量，改变速度，无摩擦力。非线性移动穿透。

方法

单位冲量引起的速度改变量

分为线速度改变量和角速度改变量

Δvunit=Δvl+Δvω\Delta v^{unit} = \Delta v_l +\Delta v_\omega

线速度改变量

Δvl=m−1\Delta v_l = m^{-1}

角速度改变量

Δω=I−1u\Delta\omega=I^{-1}u

转动惯性II，单位冲量矩u=prel×d^u=p_{rel} \times \hat{d}，碰撞点相对质心位置prel=p−op_{rel} = p - o

角速度改变量引起的速度改变量

Δvω=Δω×prel\Delta v_\omega=\Delta\omega \times p_{rel}

碰撞引起的速度改变量

碰撞前的相对速度

vrel=vl1+vω1−vl2−vω2v_{rel} = v_{l1} + v_{\omega1} - v_{l2} - v_{\omega2}

碰撞后的相对速度

vrel′=−cvrelv^\prime_{rel}=-cv_{rel}

cc为恢复系数，0~1之间，0.4像有弹性

改变量

Δvdesire=vrel′−vrel=−(1+c)vrel\Delta v^{desire} = v^\prime_{rel}-v_{rel}=-(1+c)v_{rel}

碰撞冲量

J=ΔvdesireΔvunitJ = \frac{\Delta v^{desire}}{\Delta v^{unit}}

应用冲量

Δvl=m−1J \Delta v_l = m^{-1}J
Δω=I−1u=I−1prel×J \Delta \omega = I^{-1}u=I^{-1}p_{rel} \times J

实现

void collisionResolution(PiratePhysics::CollisionShape &a, PiratePhysics::CollisionShape&b, Eigen::Vector3f &penatration)
{
    // contact point
    Vector3f contactPoint = a.localGetSupportingVertex(penatration);

    // contact world
    Vector3f contactNormal = -penatration.normalized(), y, z;
    if(abs(penatration[0]) > abs(penatration[1]))
    {
        y = {0.f, 1.0f, 0.0f};
    }
    else
    {
        y = {1.0f, 0.0f, 1.0f};
    }
    makeOrthonormalBasis(contactNormal, y, z);
    Matrix3f contactWorld;
    contactWorld.block<3, 1>(0, 0) = contactNormal;
    contactWorld.block<3, 1>(0, 1) = y;
    contactWorld.block<3, 1>(0, 2) = z;

    // velocity change per impluse
    Vector3f linearVelPer = Vector3f{1.0f, 1.0f, 1.0f}*a.getMassInv(); 
    Vector3f positionRel = contactPoint - a.getOrigin();
    Vector3f unitImpluseTorque = positionRel.cross(-penatration.normalized());
    Vector3f angularVelPer= a.getInertiaInv() * unitImpluseTorque;
    Vector3f velocityDeltaPer = linearVelPer + angularVelPer.cross(positionRel);

    // desire velocity change
    Vector3f velocityRelBefore = a.getVelocity() + b.getVelocity();
    float resitution = 0.4f;
    Vector3f velocityRelAfter = -resitution * velocityRelBefore;
    Vector3f velocityDeltaDesire = velocityRelAfter - velocityRelBefore;

    // impluse
    Vector3f impluse = velocityDeltaDesire.cwiseQuotient(velocityDeltaPer);
    Vector3f impluseTorque = positionRel.cross(impluse);

    // apply impluse
    Vector3f velocity = a.getVelocity() + a.getMassInv() * impluse;
    a.setVelocity(velocity);
    Vector3f omega = a.getOmega() + a.getInertiaInv() * impluseTorque;
    a.setOmega(omega);

    // resolving interpenetration
    Vector3f angularInertiaWorld = positionRel.cross(contactNormal);
    angularInertiaWorld = a.getInertiaInv() * angularInertiaWorld;
    angularInertiaWorld = angularInertiaWorld.cross(positionRel);
    float angularInertia = angularInertiaWorld.dot(contactNormal);
    float linearInertia = a.getMassInv();
    float totalInertia = linearInertia + angularInertia;
    float linearMove = penatration.norm() * linearInertia / totalInertia;
    float angularMove = penatration.norm() * angularInertia / totalInertia;

    // Check that the angular move is within limits.
    float angularLimitConstant = 0.2f;
    float limit = angularLimitConstant * positionRel.norm();
    if (abs(angularMove) > limit)
    {
        float totalMove = linearMove + angularMove;
        // Set the new angular move, with the same sign as before.
        if (angularMove >= 0)
        {
            angularMove = limit;
        }
        else
        {
            angularMove = -limit;
        }
        // Make the linear move take the extra slack.
        linearMove = totalMove - angularMove;
    }

    // applying movement
    Vector3f origin = a.getOrigin() + 10*contactNormal * linearMove;
    a.setOrigin(origin);
    Matrix3f inverseInertiaTensor = a.getInertiaInv();
    Vector3f impulsiveTorque = positionRel.cross(contactNormal);
    Vector3f impulsePerMove = inverseInertiaTensor * impluseTorque;
    Vector3f rotationPerMove = impulsePerMove / angularInertia;
    Vector3f rotation = rotationPerMove * angularMove;
    Matrix3f rotationMatrix;
    rotationMatrix = AngleAxisf(rotation[0], Vector3f::UnitX())
                        * AngleAxisf(rotation[1],  Vector3f::UnitY())
                        * AngleAxisf(rotation[2], Vector3f::UnitZ());
    Matrix3f rotationChange = a.getRotation() * rotationMatrix;
    a.setRotation(rotationChange);
}

测试

参考

  1. Ian Millington, 14 - Collision Resolution, Editor(s): Ian Millington, Game Physics Engine Development (Second Edition), Morgan Kaufmann, 2010, Pages 335-385, ISBN 9780123819765, doi.org/10.1016/B97…. (www.sciencedirect.com/science/art…)
avatar
文章
阅读
粉丝