One dimensional contacts test for stable contacts in physics engine (not working)

contactstest.cpp

// ONE DIMENSIONAL CONTACT TEST

// The cordinate system in the y-direction is 0 for the surface, positive above and negative below.
// Distances are measured in pixels. Time is measured in frames. So velocity = pixles / frame

// Surface line is white
// positive slop line is green
// Negative slop line is red

// Velocity threshold contact works as follows:
// * Velocity constraint is applied whenever particle is below the surface
// * Restitution is applied whenever velocity < -THRESHOLD. This means that for low velocities there is no bounce.
// * Baumgarte stabilization is applied whenever particle is below red slop line
// * Total impulse is clamped to be positive

// Positive slop constraints arent working :/

#include <SFML/Graphics.hpp>

unsigned int windowSizeX = 1600;
unsigned int windowSizeY = 1200;

// PHYSICS
const float GRAVITY = -20.0f;
const float COEFFICIENT_OF_RESTITUTION = 0.5f;

// ACTIVE - INACTIVE CONSTRAINT COLORS
const sf::Color ACTIVE_CONTACT_COLOR(255, 0, 255);
const sf::Color INACTIVE_CONTACT_COLOR(255, 255, 255);

// SLOP
const float SLOP = windowSizeY * 0.01f;
const float POSITIVE_SLOP = windowSizeY * 0.1f;

// VELOCITY THRESHOLD
const float VELOCITY_THRESHOLD = GRAVITY * -2.0f;

// COORDINATES
const float Y_ZERO = windowSizeY * 3.0f / 4.0f;
const float SURFACE_LINE_Y = 0;
const float SLOP_LINE_Y = -SLOP;
const float POSITIVE_SLOP_LINE_Y = POSITIVE_SLOP;

// CONTACTS
struct Contact {
    float positionX;
    float positionY;
    float velocity = 0.0f;
};

Contact velocityThresholdConstraint = { windowSizeX * 1.0f / 3.0f, windowSizeY / 2.0f, 0.0f };
Contact positiveSlopConstraint = { windowSizeX * 2.0f / 3.0f, windowSizeY / 2.0f, 0.0f };

// DRAWING
sf::RenderWindow window;

sf::RectangleShape lineShape(sf::Vector2f(windowSizeX, 1.0f));
void drawLine(const float y, const sf::Color color) {
    lineShape.setPosition(0.0f, Y_ZERO - y);
    lineShape.setFillColor(color);
    window.draw(lineShape);
}

sf::CircleShape contactShape(4.0f);
void drawContact(const float x, const float y, const sf::Color color) {
    contactShape.setPosition(x, Y_ZERO - y - contactShape.getRadius() / 2.0f);
    contactShape.setFillColor(color);
    window.draw(contactShape);
}

// EVENTS
void doEvents() {
    sf::Event event;
    while (window.pollEvent(event)) {
        if (event.type == sf::Event::Closed) {
            window.close();
        }
    }
}

// MAIN
int main() {
    
    window.create(sf::VideoMode(windowSizeX, windowSizeY), "Solver Stability");
    
    while (window.isOpen()) {
        doEvents();

        // GATHER CONTACTS
        bool velocityThresholdConstraintIsActive = velocityThresholdConstraint.positionY < 0;
        bool positiveSlopConstraintIsActive = positiveSlopConstraint.positionY < POSITIVE_SLOP;

        // INTEGRATE FORCES
        velocityThresholdConstraint.velocity += GRAVITY;
        positiveSlopConstraint.velocity += GRAVITY;

        // SOLVE VELOCITY THRESHOLD CONSTRAINT
        if (velocityThresholdConstraintIsActive) {
            float restitution = (velocityThresholdConstraint.velocity < -VELOCITY_THRESHOLD) ? velocityThresholdConstraint.velocity * COEFFICIENT_OF_RESTITUTION : 0.0f;
            float baumgarte = std::min(velocityThresholdConstraint.positionY + SLOP, 0.0f) * 0.1f;
            float impulse = std::max(-(velocityThresholdConstraint.velocity + restitution + baumgarte), 0.0f);
            velocityThresholdConstraint.velocity += impulse;
        }
        
        // SOLVE POSITIVE SLOP CONSTRAINT
        if (positiveSlopConstraintIsActive) {
            float restitution = positiveSlopConstraint.velocity * COEFFICIENT_OF_RESTITUTION;
            float baumgarte = std::min(positiveSlopConstraint.positionY, 0.0f) * 0.1f;
            float impulse = std::max(-(positiveSlopConstraint.velocity + restitution + baumgarte), 0.0f);
            positiveSlopConstraint.velocity += impulse;
        }
        
        // INTEGRATE VELOCITIES
        velocityThresholdConstraint.positionY += velocityThresholdConstraint.velocity;
        positiveSlopConstraint.positionY += positiveSlopConstraint.velocity;

        // DRAW
        window.clear(sf::Color::Black);

        drawLine(SURFACE_LINE_Y, sf::Color(255, 255, 255));
        drawLine(SLOP_LINE_Y, sf::Color(255, 0, 0));
        drawLine(POSITIVE_SLOP_LINE_Y, sf::Color(0, 255, 0));
        
        sf::Color velocityThresholdConstraintColor = velocityThresholdConstraintIsActive ? ACTIVE_CONTACT_COLOR : INACTIVE_CONTACT_COLOR;
        drawContact(velocityThresholdConstraint.positionX, velocityThresholdConstraint.positionY, velocityThresholdConstraintColor);

        sf::Color positiveSlopConstraintColor = positiveSlopConstraintIsActive ? ACTIVE_CONTACT_COLOR : INACTIVE_CONTACT_COLOR;
        drawContact(positiveSlopConstraint.positionX, positiveSlopConstraint.positionY, positiveSlopConstraintColor);

        // DISPLAY
        window.display();
        
        // SLEEP
        sf::sleep(sf::milliseconds(100));
    }

    return 0;
}