Tenho esse código de um motor de física 2D simples. O problema é que os objetos parecem tremer e se mover lentamente quando é para estarem em repouso. Essa parte parece ser onde o calculo do atrito é feito

Código C++:
 "void PenetrationConstraint::PreSolve(const float dt) {
    // Get the collision points and normal in world space
    const Vec2 pa = a->LocalSpaceToWorldSpace(aPoint);
    const Vec2 pb = b->LocalSpaceToWorldSpace(bPoint);
    Vec2 n = a->LocalSpaceToWorldSpace(normal);

    const Vec2 ra = pa - a->position;
    const Vec2 rb = pb - b->position;

    jacobian.Zero();

    // Populate the first row of the Jacobian (normal vector)
    jacobian.rows[0][0] = -n.x;         // A linear velocity.x
    jacobian.rows[0][1] = -n.y;         // A linear velocity.y
    jacobian.rows[0][2] = -ra.Cross(n); // A angular velocity
    jacobian.rows[0][3] = n.x;          // B linear velocity.x
    jacobian.rows[0][4] = n.y;          // B linear velocity.y
    jacobian.rows[0][5] = rb.Cross(n);  // B angular velocity

    // Populate the second row of the Jacobian (tangent vector)
    friction = std::max(a->friction, b->friction);
    if (friction > 0.0) {
        Vec2 t = n.Normal(); // The tangent is the vector perpendicular to the normal
        jacobian.rows[1][0] = -t.x;          // A linear velocity.x
        jacobian.rows[1][1] = -t.y;          // A linear velocity.y
        jacobian.rows[1][2] = -ra.Cross(t);  // A angular velocity
        jacobian.rows[1][3] = t.x;           // B linear velocity.x
        jacobian.rows[1][4] = t.y;           // B linear velocity.y
        jacobian.rows[1][5] = rb.Cross(t);   // B angukar velocity
    }

    // Warm starting (apply cached lambda)
    const MatMN Jt = jacobian.Transpose();
    VecN impulses = Jt * cachedLambda;

    // Apply the impulses to both bodies 
    a->ApplyImpulseLinear(Vec2(impulses[0], impulses[1])); // A linear impulse
    a->ApplyImpulseAngular(impulses[2]);                   // A angular impulse
    b->ApplyImpulseLinear(Vec2(impulses[3], impulses[4])); // B linear impulse
    b->ApplyImpulseAngular(impulses[5]);                   // B angular impulse

    // Compute the bias term (baumgarte stabilization)
    const float beta = 0.2f;
    float C = (pb - pa).Dot(-n);
    C = std::min(0.0f, C + 0.01f);
    bias = (beta / dt) * C;
}

void PenetrationConstraint::Solve() {
    const VecN V = GetVelocities();
    const MatMN invM = GetInvM();
  
    const MatMN J = jacobian;
    const MatMN Jt = jacobian.Transpose();
    
    // Compute lambda using Ax=b (Gauss-Seidel method) 
    MatMN lhs = J * invM * Jt;  // A
    VecN rhs = J * V * -1.0f;   // b
    rhs[0] -= bias;
    VecN lambda = MatMN::SolveGaussSeidel(lhs, rhs);
    
    // Accumulate impulses and clamp it within constraint limits
    VecN oldLambda = cachedLambda;
    cachedLambda += lambda;
    cachedLambda[0] = (cachedLambda[0] < 0.0f) ? 0.0f : cachedLambda[0];

    // Keep friction values between -(λn*µ) and (λn*µ)
	if (friction > 0.0) {
		const float maxFriction = cachedLambda[0] * friction;
        cachedLambda[1] = std::clamp(cachedLambda[1], -maxFriction, maxFriction);
	}
    
    lambda = cachedLambda - oldLambda;

    // Compute the impulses with both direction and magnitude
    VecN impulses = Jt * lambda;

    // Apply the impulses to both bodies 
    a->ApplyImpulseLinear(Vec2(impulses[0], impulses[1])); // A linear impulse
    a->ApplyImpulseAngular(impulses[2]);                   // A angular impulse
    b->ApplyImpulseLinear(Vec2(impulses[3], impulses[4])); // B linear impulse
    b->ApplyImpulseAngular(impulses[5]);                   // B angular impulse
}"