c++game-physicsphysics-engine

Rigidbody sliding off when distance between another rigidbody is greater than zero in C++ physics engine


I'm trying to make a physics engine in C++ and I have a rigidbody script and a function for resolving collisions. But when the distance between one rigidbody and another becomes slightly larger, the rigidbody with less mass just slides off. I do not want this ofcourse. Please help me. This is my function for resolving collisions:

void ResolveCollisions(Rigidbody& rigidbody, Rigidbody& boxRigidbody)
{
    float distance = glm::distance(rigidbody.getPosition(), boxRigidbody.getPosition());
    std::cout << distance;

    // Calculate minimum penetration depth based on combined radius/bounding box half size
    float minimumPenetrationDepth = rigidbody.getMass() + boxRigidbody.getMass();

    if (distance - minimumPenetrationDepth <= 0.01f)
    {
        glm::vec3 collisionNormal = glm::normalize(rigidbody.getPosition() - boxRigidbody.getPosition());

        // Calculate relative velocity
        glm::vec3 relativeVelocity = rigidbody.getVelocity() - boxRigidbody.getVelocity();
        float relativeVelocityNormal = glm::dot(relativeVelocity, collisionNormal);

        float restitution = 0.1f; // Adjust the coefficient as needed

        // Calculate impulse magnitude for normal direction
        float j = -(1 + restitution) * relativeVelocityNormal;
        j /= 1 / rigidbody.getMass() + 1 / boxRigidbody.getMass();

        // Apply impulse for normal direction
        glm::vec3 impulse = j * collisionNormal;

        // Update velocities for normal direction
        rigidbody.setVelocity(rigidbody.getVelocity() + impulse / rigidbody.getMass());

        // Resolve penetration
        (rigidbody.getMass() + boxRigidbody.getMass()); // Use combined mass for center of mass calculation
        const float percent = 0.2f; // Penetration percentage to correct
        const float slop = 0.1f; // Allowance to prevent jittering
        float penetrationDepth = calculatePenetrationDepth(rigidbody, boxRigidbody);
        glm::vec3 desiredDistance =
            0.5f * (rigidbody.getBoundingBoxMax() - rigidbody.getBoundingBoxMin()) +
            0.5f * (boxRigidbody.getBoundingBoxMax() - boxRigidbody.getBoundingBoxMin());; // Calculate desired non-penetration distance (e.g., sum of bounding box half sizes)
        float desiredDistanceMagnitude = glm::length(desiredDistance);
        float penetrationDepthBruh = desiredDistanceMagnitude - distance;

        if (penetrationDepthBruh > slop) {
            glm::vec3 correction = penetrationDepth * collisionNormal;
            rigidbody.setPosition(rigidbody.getPosition() + correction);
        }

        // Calculate relative velocity in the direction of the tangent (friction)
        glm::vec3 relativeVelocityTangent = relativeVelocity - (glm::dot(relativeVelocity, collisionNormal) * collisionNormal);
        float relativeVelocityTangentMagnitude = glm::length(relativeVelocityTangent);

        // Calculate friction coefficient
        float staticFrictionThreshold = 0.001f;
        float frictionCoefficient = 0.1f;

        // Apply friction impulse if there's relative tangential velocity
        if (relativeVelocityTangentMagnitude < staticFrictionThreshold) {
            // If relative tangential velocity is low, apply static friction to prevent sliding
            // Calculate static friction impulse
            glm::vec3 staticFrictionImpulseA = -relativeVelocityTangent * rigidbody.getMass(); // Opposes motion
            glm::vec3 staticFrictionImpulseB = -relativeVelocityTangent * boxRigidbody.getMass(); // Opposes motion

            // Apply static friction impulse
            rigidbody.setVelocity(rigidbody.getVelocity() + staticFrictionImpulseA / rigidbody.getMass());
        }
        else {
            // If relative tangential velocity is high, apply dynamic friction
            // Calculate friction coefficient
            float frictionCoefficient = 0.1f; // Adjust as needed

            // Apply friction impulse if there's relative tangential velocity
            // Calculate impulse magnitude for friction
            float frictionImpulseMagnitude = frictionCoefficient * j;

            // Clamp friction impulse magnitude to prevent reversal of relative motion
            frictionImpulseMagnitude = std::min(frictionImpulseMagnitude, relativeVelocityTangentMagnitude);

            // Calculate friction impulse vector
            glm::vec3 frictionImpulse = glm::normalize(relativeVelocityTangent) * frictionImpulseMagnitude;

            // Apply friction impulse
            rigidbody.setVelocity(rigidbody.getVelocity() - frictionImpulse / rigidbody.getMass());
        }

        // Calculate angular velocity change due to collision
        glm::vec3 rA = rigidbody.getPosition() - boxRigidbody.getPosition();
        glm::vec3 rB = boxRigidbody.getPosition() - rigidbody.getPosition();
        glm::vec3 angularVelocityChangeA = glm::cross(rA, impulse) / rigidbody.getMass();
        glm::vec3 angularVelocityChangeB = glm::cross(rB, -impulse) / boxRigidbody.getMass();

        // Apply angular velocity change
        rigidbody.setRotation(rigidbody.getRotation() + angularVelocityChangeA);
    }
}

And if you were wondering, this is my rigidbody script:

class Rigidbody {
private:
    glm::vec3 position;
    glm::vec3 velocity;
    glm::vec3 acceleration;
    glm::vec3 force;
    float mass;
    glm::vec3 gravity;
    glm::vec3 rotation;
 
    // Bounding box dimensions
    glm::vec3 boundingBoxMin;
    glm::vec3 boundingBoxMax;
    glm::vec3 colliderRotation;
 
    float radius;
 
public:
    Rigidbody(glm::vec3 initialPosition, float initialMass, glm::vec3 initialGravity,
        Model& model, glm::vec3 initialRotation)
        : position(initialPosition), mass(initialMass), velocity(glm::vec3(0.0f)),
        acceleration(glm::vec3(0.0f)), force(glm::vec3(0.0f)), gravity(initialGravity),
        rotation(initialRotation), colliderType(ColliderType::BoundingBox) {
        // Get bounding box from the model
        boundingBoxMax = model.GetMaxBoundingBox();
        boundingBoxMin = model.GetMinBoundingBox();
    }
 
    // Constructor for spherical collider
    Rigidbody(glm::vec3 initialPosition, float initialMass, glm::vec3 initialGravity,
        float initialRadius)
        : position(initialPosition), mass(initialMass), velocity(glm::vec3(0.0f)),
        acceleration(glm::vec3(0.0f)), force(glm::vec3(0.0f)), gravity(initialGravity),
        rotation(glm::vec3(0.0f)), radius(initialRadius), colliderType(ColliderType::Sphere) {}
 
    void applyForce(glm::vec3 externalForce) {
        force += externalForce;
    }
 
    void update(float deltaTime) {
        // Apply gravity
        force += mass * gravity;
 
        // Apply Newton's second law: F = ma
        acceleration = force / mass;
 
        // Update velocity
        velocity += acceleration * deltaTime;
 
        // Update position
        position += velocity * deltaTime;
 
        // Reset force for next update
        force = glm::vec3(0.0f);
 
    }
 
    //here would be all the getters and setters
 
    enum class ColliderType {
        BoundingBox,
        Sphere
    } colliderType;
 
    // Methods to get the bounding box dimensions
    glm::vec3 getBoundingBoxMin() const {
        // Apply rotation to the min bounding box coordinates
        glm::vec3 rotatedMin = boundingBoxMin;
        rotatedMin = glm::rotateX(rotatedMin, colliderRotation.x);
        rotatedMin = glm::rotateY(rotatedMin, colliderRotation.y);
        rotatedMin = glm::rotateZ(rotatedMin, colliderRotation.z);
        return rotatedMin;
    }
 
    glm::vec3 getBoundingBoxMax() const {
        // Apply rotation to the max bounding box coordinates
        glm::vec3 rotatedMax = boundingBoxMax;
        rotatedMax = glm::rotateX(rotatedMax, colliderRotation.x);
        rotatedMax = glm::rotateY(rotatedMax, colliderRotation.y);
        rotatedMax = glm::rotateZ(rotatedMax, colliderRotation.z);
        return rotatedMax;
    }
 
    // Bounding box collision detection
    bool checkCollision(const Rigidbody& other) const {
        // Get the bounding box of the other Rigidbody
        glm::vec3 minB = other.getPosition() + other.getBoundingBoxMin();
        glm::vec3 maxB = other.getPosition() + other.getBoundingBoxMax();
 
        // Check if this bounding box intersects with the other bounding box
        return (position.x + boundingBoxMax.x >= minB.x &&
            position.x + boundingBoxMin.x <= maxB.x &&
            position.y + boundingBoxMax.y >= minB.y &&
            position.y + boundingBoxMin.y <= maxB.y &&
            position.z + boundingBoxMax.z >= minB.z &&
            position.z + boundingBoxMin.z <= maxB.z);
    }
};
 

Solution

  • 1. A few methods are missing:

    I assume you just left them out to make things easier, but then you have to assume the following:

    Rigidbody.getPosition()” returns the variable “glm::vec3 position”.

    Rigidbody.getMass()” returns the variable “float mass;”.

    Rigidbody.setVelocity(glm::vec3)” sets the variable “glm::vec3 velocity;”.

    what does “calculatePenetrationDepth(rigidbody, boxRigidbody)” do?

    2. what is “rigidbody.position”?!

    I assume that is the center point of the rigidbody... but what kind of center (center of mass or center of volume) I don't know...

    3. getBoundingBoxMin() / -Max()

    Here you have to be careful that these methods do not return the real minimum / maximum. It may be that “getBoundingBoxMax()” is smaller (in all dimensions) than “getBoundingBoxMin()” Therefore, I would suggest that it is either internally calculated in the methods, or that it is named “getBoundingBox1()” and “getBoundingBox2()

    4. “checkCollision(const Rigidbody& other)”

    This method only works with AABB and here again we have the 3rd problem...

    5. first if-statement

    if (distance - minimumPenetrationDepth <= 0.01f)” here you should still include the bounding box in the distance of the objects, because with low mass, very high volume and large distance, no collision is calculated. Or if the mass is very high and the volume is low, a collision is calculated that does not need to be calculated.

    6. unused calculation

    (rigidbody.getMass() + boxRigidbody.getMass()); no comment...

    7. friction

    perhaps you have simply forgotten to take the friction of the base into account, and also because of problem 5, a force is calculated that is normally less than the inertia and friction of the body