CollisionSystem.cpp

#include <math.h>
#include <iostream>
#include "CollisionSystem.h"
#include "ECS.h"
#include "GameData.h"
#include "CONFIG.h"
#include "EnabledSystem.h"
#include "PositionSystem.h"
#include "VelocitySystem.h"
#include "BoundsSystem.h"

namespace station {
    CollisionSystem::CollisionSystem(GameData* DATA) : System(DATA) {

    }
    CollisionSystem::~CollisionSystem() {

    }

    void CollisionSystem::init() {

    }
    void CollisionSystem::update(float dt) {
        //set all grounded flags to false first
        for (int i = 0; i < components.size(); i++) {
            components.at(i)->groundedOnEntity = false;
        }
        for (int i = 0; i < components.size(); i++) {
            for (int i2 = i; i2 < components.size(); i2++) {
                if (!DEBUG_DISABLECOLLISION && i != i2) {
                    IFDISABLED(components.at(i)->getCompID())
                        IFDISABLED(components.at(i2)->getCompID()) continue;

                    if (components.at(i)->staticCollider && components.at(i2)->staticCollider) continue;

                    pos1 = data->ecs->positionSystem->getComp(components.at(i)->getCompID());
                    pos2 = data->ecs->positionSystem->getComp(components.at(i2)->getCompID());
                    bounds1 = data->ecs->boundsSystem->getComp(components.at(i)->getCompID());
                    bounds2 = data->ecs->boundsSystem->getComp(components.at(i2)->getCompID());
                    v1 = data->ecs->velocitySystem->getComp(components.at(i)->getCompID());
                    v2 = data->ecs->velocitySystem->getComp(components.at(i2)->getCompID());

                    if (COLLISIONSYSTEM_EXCESSIVEDEBUG && !COLLISIONSYSTEM_IGNOREII) std::cout << "Checking intersections:\n";
                    //detect collision
                    IntersectionInfo ii = detectCollision(i, i2);
                    //collision detected?
                    if (ii.collided) {
                        //resolve collision
                        resolveCollision(ii, i, i2);
                        correctPosition(ii, i, i2);
                    }
                }
            }
        }
    }
    void CollisionSystem::draw(sf::RenderTarget &target, sf::RenderStates states) const {
        if (DEBUG_DRAWCOLLIDERS) {
            sf::Vertex line[4];
            for (CollisionComponent* v : components) {
                IFDISABLED(v->getCompID()) continue;
                PositionComponent* p = data->ecs->positionSystem->
                    getComp(v->getCompID());
                BoundsComponent* b = data->ecs->boundsSystem->
                    getComp(v->getCompID());
                sf::Color crossColor;
                if (v->staticCollider) crossColor = sf::Color(255, 0, 0);
                else crossColor = sf::Color(0, 255, 0);
                //top left to bottom right
                line[0].position.x = p->position.x - (b->bounds.x / 2);
                line[0].position.y = p->position.y - (b->bounds.y / 2);
                line[0].color = crossColor;
                line[1].position.x = p->position.x + (b->bounds.x / 2);
                line[1].position.y = p->position.y + (b->bounds.y / 2);
                line[1].color = crossColor;
                //bottom left to top right
                line[2].position.x = p->position.x - (b->bounds.x / 2);
                line[2].position.y = p->position.y + (b->bounds.y / 2);
                line[2].color = crossColor;
                line[3].position.x = p->position.x + (b->bounds.x / 2);
                line[3].position.y = p->position.y - (b->bounds.y / 2);
                line[3].color = crossColor;
                target.draw(line, (sizeof(line) / sizeof(*line)), sf::Lines);
            }
        }
    }


#pragma region collision detection
    IntersectionInfo CollisionSystem::detectCollision(int componentat1, int componentat2) {
        if (COLLISIONSYSTEM_EXCESSIVEDEBUG && !COLLISIONSYSTEM_IGNOREII)
            std::cout << "Testing intersection between entity " << componentat1 << " and " << componentat2 << '\n';

        IntersectionInfo ii;

        //calculate directional vector from collider 1 to 2
        sf::Vector2f dir(pos1->position.x - pos2->position.x, pos1->position.y - pos2->position.y);
        /*this is the part where im admitting that i tried to do collision myself and utterly failed
        as one can see in the CollisionComponent.cppold file. This code is closely following the code
        showed by Randy Gaul on his AABB collision tutsplus blogpost. https://goo.gl/5q4DPN */
        //overlap between the two colliders in the x axis
        float x_overlap = (bounds1->bounds.x * 0.5f) + (bounds2->bounds.x * 0.5f) - abs(dir.x);
        //overlap between the two colliders in the y axis
        float y_overlap = (bounds1->bounds.y * 0.5f) + (bounds2->bounds.y * 0.5f) - abs(dir.y);
        if (false || (COLLISIONSYSTEM_EXCESSIVEDEBUG && !COLLISIONSYSTEM_IGNOREII))
            std::cout << "x_overlap: " << x_overlap << ", y_overlap: " << y_overlap << '\n';

        if (x_overlap > 0 && y_overlap > 0) {

            //std::cout << v1 << " " << v2 << "\n";

            //collision!
            ii.collided = true;
            //calculate collision normal (determines in what direction to apply impulse)
            //penetrationDepth is just the least amount of overlap (either x or y axis)
            /*x_over_y_bias helps with small steps etc.*/
            if (x_overlap < y_overlap) {
                if (dir.x < 0) {
                    ii.collisionNormal = sf::Vector2f(-1.0f, 0.0f);
                }
                else {
                    ii.collisionNormal = sf::Vector2f(1.0f, 0.0f);
                }
                ii.penetrationDepth = x_overlap;
            }
            else {
                if (dir.y < 0) {
                    ii.collisionNormal = sf::Vector2f(0.0f, -1.0f);
                    components.at(componentat1)->groundedOnEntity = true;
                    components.at(componentat1)->entityGroundedOn =
                        components.at(componentat2);
                }
                else {
                    ii.collisionNormal = sf::Vector2f(0.0f, 1.0f);
                    components.at(componentat2)->groundedOnEntity = true;
                    components.at(componentat2)->entityGroundedOn =
                        components.at(componentat1);
                }
                ii.penetrationDepth = y_overlap;
            }
            if (false || (COLLISIONSYSTEM_EXCESSIVEDEBUG && !COLLISIONSYSTEM_IGNOREII))
                std::cout << "Collision between entity " << componentat1 << " and " << componentat2 << ":\n"
                << "CNormal: (" << ii.collisionNormal.x << ", " << ii.collisionNormal.y <<
                "), PDepth: " << ii.penetrationDepth << '\n';
        }
        else {
            //no collision :-(
            ii.collided = false;
            if (COLLISIONSYSTEM_EXCESSIVEDEBUG && !COLLISIONSYSTEM_IGNOREII)
                std::cout << "No collision between entity " << componentat1 << " and " << componentat2 << ".\n";
        }

        return ii;
    }
#pragma endregion
#pragma region resolve Collision
    void CollisionSystem::resolveCollision(IntersectionInfo ii, int componentat1, int componentat2) {
        //calc inverse masses
        invMass1 = components.at(componentat1)->staticCollider ? 0 : 1 / components.at(componentat1)->mass;
        invMass2 = components.at(componentat2)->staticCollider ? 0 : 1 / components.at(componentat2)->mass;

        //calculate relative velocity
        sf::Vector2f relativeVelocity(
            (v1 ? v1->velocity : sf::Vector2f()) -
            (v2 ? v2->velocity : sf::Vector2f())
        );
        //calculate relative velocity in terms of the normal direction. omegalul
        float velAlongNormal =
            relativeVelocity.x * ii.collisionNormal.x +
            relativeVelocity.y * ii.collisionNormal.y;

        //dont resolve if velocities are seperating (if the colliders are moving apart)
        if (velAlongNormal > 0) return;

        //calculate restitution (bounciness)
        float restitution = std::min(
            components.at(componentat1)->restitution,
            components.at(componentat2)->restitution
        );

        //calculate impulse scalar (how big the pushy force out do)
        float j = -(1 + ii.penetrationDepth) * velAlongNormal;
        j /= invMass1 + invMass2;

        //apply impulse
        sf::Vector2f impulse = ii.collisionNormal * j; //collision direction times the impulse force j
        if(v1) v1->velocity += invMass1 * impulse;
        if(v2) v2->velocity -= invMass2 * impulse;
    }
#pragma endregion
#pragma region correct position
    void CollisionSystem::correctPosition(IntersectionInfo ii, int componentat1, int componentat2) {
        sf::Vector2f correction =
            std::max(ii.penetrationDepth - COLLISIONSYSTEM_SLOP, 0.0f) /
            (invMass1 + invMass2) *
            COLLISIONSYSTEM_PROJECTIONPERCENTAGE *
            ii.collisionNormal;
        pos1->position += invMass1 * correction;
        pos2->position -= invMass2 * correction;
    }
#pragma endregion
}