Terrible Physics...

#include <iostream>
#include <vector>
#include <math.h>
#include "SDL/SDL.h"
#include "SDL/SDL_gfxPrimitives.h"

#define WIDTH 320
#define HEIGHT 240

#define FPS 60
#define SPF 5

using namespace std;

struct Dimension
{
    public:
        double x;
        double y;

        Dimension();
        Dimension(double x);
        Dimension(double x, double y);

        void setValue(double x, double y);
        void reset();

        Dimension unit();

        double length();
        double distance(Dimension operend);
        double dotProduct(Dimension operend);

        Dimension operator +(Dimension operend);
        Dimension operator -(Dimension operend);
        Dimension operator *(Dimension operend);
        Dimension operator /(Dimension operend);
        Dimension operator *(double operend);
        Dimension operator /(double operend);

        bool operator ==(Dimension operend);
        bool operator !=(Dimension operend);
        bool operator >(Dimension operend);
        bool operator <(Dimension operend);
        bool operator >=(Dimension operend);
        bool operator <=(Dimension operend);
};

Dimension::Dimension()
{
    this->x = 0;
    this->y = 0;
}

Dimension::Dimension(double x)
{
    this->x = x;
    this->y = x;
}

Dimension::Dimension(double x, double y)
{
    this->x = x;
    this->y = y;
}

void Dimension::setValue(double x, double y)
{
    this->x = x;
    this->y = y;
}

void Dimension::reset()
{
    x = 0;
    y = 0;
}

Dimension Dimension::unit()
{
    return *this / length();
}

double Dimension::length()
{
    return sqrt(pow(x, 2) + pow(y, 2));
}

double Dimension::distance(Dimension operend)
{
    Dimension difference = operend - *this;
    return difference.length();
}

double Dimension::dotProduct(Dimension operend)
{
    return x * operend.x + y * operend.y;
}

Dimension Dimension::operator +(Dimension operend)
{
    Dimension result = Dimension(x, y);
    result.x += operend.x;
    result.y += operend.y;
    return result;
}

Dimension Dimension::operator -(Dimension operend)
{
    Dimension result = Dimension(x, y);
    result.x -= operend.x;
    result.y -= operend.y;
    return result;
}

Dimension Dimension::operator *(Dimension operend)
{
    Dimension result = Dimension(x, y);
    result.x *= operend.x;
    result.y *= operend.y;
    return result;
}

Dimension Dimension::operator /(Dimension operend)
{
    Dimension result = Dimension(x, y);
    result.x /= operend.x;
    result.y /= operend.y;
    return result;
}

Dimension Dimension::operator *(double operend)
{
    Dimension result = Dimension(x, y);
    result.x *= operend;
    result.y *= operend;
    return result;
}

Dimension Dimension::operator /(double operend)
{
    Dimension result = Dimension(x, y);
    result.x /= operend;
    result.y /= operend;
    return result;
}

bool Dimension::operator ==(Dimension operend)
{
    return x == operend.x && y == operend.y;
}

bool Dimension::operator !=(Dimension operend)
{
    return x != operend.x || y != operend.y;
}

bool Dimension::operator >(Dimension operend)
{
    return x > operend.x && y > operend.y;
}

bool Dimension::operator <(Dimension operend)
{
    return x < operend.x && y < operend.y;
}

bool Dimension::operator >=(Dimension operend)
{
    return (x > operend.x && y > operend.y) || ((*this) == operend);
}

bool Dimension::operator <=(Dimension operend)
{
    return (x < operend.x && y < operend.y) || ((*this) == operend);
}

class Entity
{
    public:
        Entity();
        ~Entity();

        void update();
        void render(SDL_Surface *surface);

        void keyDown(SDL_keysym key);
        void keyUp(SDL_keysym key);
        void mouseDown(unsigned short button, Dimension position);
        void mouseUp(unsigned short button, Dimension position);
        void mouseMotion(Dimension position, Dimension motion);

        Dimension getPosition();
        Dimension getSize();

        unsigned long getBackgroundColor();

        void setParent(Entity *parent);

    protected:
        long getTick();
        long getFrame();

        void addEntity(Entity *entity);
        void removeEntity(Entity *entity);
        void removeEntity(int entity);
        void clearEntities();

        void bringToFront(Entity *entity);
        void bringToFront();

        void remove();

        void setFocused(bool focused);
        bool isFocused();

        virtual void update2() = 0;
        virtual void render2(SDL_Surface *surface) = 0;

        virtual void onKeyDown(SDL_keysym key) {}
        virtual void onKeyUp(SDL_keysym key) {}
        virtual void onMouseDown(unsigned short button, Dimension position) {}
        virtual void onMouseUp(unsigned short button, Dimension position) {}
        virtual void onMouseMotion(Dimension position, Dimension motion) {}
        virtual void onClick(unsigned short button, Dimension position) {}
        virtual void onDrag(unsigned short button, Dimension position, Dimension motion) {}
        virtual void onFocus() {}
        virtual void onUnfocus() {}

        Entity *getEntityInBounds(Dimension bounds);

        Dimension position;
        Dimension size;

        unsigned long backgroundColor;

        Entity *getParent();

        vector<Entity *> contents;

        bool mouse;
        unsigned short button;
        Dimension mousePosition;

    private:
        Entity *focus;

        bool focused;

        long tick;
        long frame;

        Entity *parent;
};

Entity::Entity()
{
    contents = vector<Entity *>();
    focus = NULL;

    mouse = false;
    button = 0;

    focused = false;

    tick = 0;
    frame = 0;

    position = Dimension(0, 0);
    size = Dimension(0, 0);

    backgroundColor = 0x000000;

    parent = NULL;
}

Entity::~Entity()
{
    clearEntities();
}

void Entity::update()
{
    update2();
    tick++;

    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        (*it)->update();
    }
}

void Entity::render(SDL_Surface *surface)
{
    SDL_Surface *surface2 = SDL_CreateRGBSurface(SDL_HWSURFACE, size.x, size.y, 32, 0x00FF0000, 0x0000FF00, 0x000000FF, 0x00000000);

    render2(surface2);
    frame++;

    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        (*it)->render(surface2);
    }

    SDL_Rect offset;
    offset.x = position.x;
    offset.y = position.y;

    SDL_BlitSurface(surface2, NULL, surface, &offset);
    SDL_FreeSurface(surface2);
}

void Entity::keyDown(SDL_keysym key)
{
    onKeyDown(key);

    if(focus != NULL)
    {
        focus->onKeyDown(key);
    }
}

void Entity::keyUp(SDL_keysym key)
{
    onKeyUp(key);

    if(focus != NULL)
    {
        focus->onKeyUp(key);
    }
}

void Entity::mouseDown(unsigned short button, Dimension position)
{
    mouse = true;
    this->button = button;
    mousePosition = position;
    onMouseDown(button, position);

    Entity *oldFocus = focus;
    focus = getEntityInBounds(position);

    if(focus != oldFocus)
    {
        if(focus != NULL)
        {
            focus->setFocused(true);
        }
        if(oldFocus != NULL)
        {
            oldFocus->setFocused(false);
        }
    }

    if(focus != NULL)
    {
        focus->onMouseDown(button, position);
    }
}

void Entity::mouseUp(unsigned short button, Dimension position)
{
    mouse = false;
    mousePosition = position;

    Entity *focus = getEntityInBounds(position);

    onMouseUp(button, position);
    if(mouse)
    {
        onClick(button, position);

        if(focus != NULL && focus == this->focus)
        {
            focus->onClick(button, position);
        }
    }

    if(focus != NULL)
    {
        focus->onMouseUp(button, position);
    }
}

void Entity::mouseMotion(Dimension position, Dimension motion)
{
    mousePosition = position;

    onMouseMotion(position, motion);
    if(mouse)
    {
        onDrag(button, position, motion);

        if(focus != NULL)
        {
            focus->onDrag(button, position, motion);
        }
    }

    Entity *focus = getEntityInBounds(position);

    if(focus != NULL)
    {
        focus->onMouseMotion(position, motion);
    }
}

Entity *Entity::getParent()
{
    return parent;
}

void Entity::setParent(Entity *parent)
{
    this->parent = parent;
}

Dimension Entity::getPosition()
{
    return position;
}

Dimension Entity::getSize()
{
    return size;
}

unsigned long Entity::getBackgroundColor()
{
    return backgroundColor;
}

long Entity::getTick()
{
    return tick;
}

long Entity::getFrame()
{
    return frame;
}

void Entity::addEntity(Entity *entity)
{
    contents.push_back(entity);
    entity->setParent(this);
}

void Entity::removeEntity(Entity *entity)
{
    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        if(entity == *it)
        {
            contents.erase(it);
            break;
        }
    }
}

void Entity::removeEntity(int entity)
{
    contents.erase(contents.begin() + entity);
}

void Entity::clearEntities()
{
    contents.clear();
}

void Entity::bringToFront(Entity *entity)
{
    removeEntity(entity);
    addEntity(entity);
}

void Entity::bringToFront()
{
    if(parent != NULL)
    {
        parent->bringToFront(this);
    }
}

void Entity::remove()
{
    if(parent != NULL)
    {
        parent->removeEntity(this);
    }
}

void Entity::setFocused(bool focused)
{
    this->focused = focused;
    if(focused)
    {
        onFocus();
    }
    else
    {
        onUnfocus();
    }
}

bool Entity::isFocused()
{
    return focused;
}

Entity *Entity::getEntityInBounds(Dimension position)
{
    if(contents.size())
    {
        vector<Entity *>::iterator it;
        for(it = contents.end() - 1; it >= contents.begin(); it--)
        {
            Entity *entity = *it;
            if(position >= entity->getPosition() && position < entity->getPosition() + entity->getSize())
            {
                return entity;
            }
        }
    }

    return NULL;
}

class PhysicalEntity: public Entity
{
    public:
        PhysicalEntity();
        ~PhysicalEntity();

        void applyForce(Dimension force);
        virtual void collide(PhysicalEntity *entity) = 0;

        double getMass();
        double getSpeed();
        Dimension getVelocity();

    protected:
        void update2();

        virtual void update3() {};
        virtual void applyForces() {};

        Dimension velocity;
        Dimension forces;

        double mass;
};

PhysicalEntity::PhysicalEntity()
{
    velocity = Dimension(0, 0);
    forces = Dimension(0, 0);

    mass = 1;
}

PhysicalEntity::~PhysicalEntity()
{

}

void PhysicalEntity::applyForce(Dimension force)
{
    forces = forces + force;
}

double PhysicalEntity::getMass()
{
    return mass;
}

double PhysicalEntity::getSpeed()
{
    return velocity.length();
}

Dimension PhysicalEntity::getVelocity()
{
    return velocity;
}

void PhysicalEntity::update2()
{
    update3();

    applyForces();

    velocity = velocity + forces / mass;
    forces.reset();
    position = position + velocity;
}

class Point: public PhysicalEntity
{
    public:
        Point(Dimension position);
        ~Point();

        void collide(PhysicalEntity *entity);

    protected:
        void render2(SDL_Surface *surface);
};

Point::Point(Dimension position)
{
    this->position = position;
    size = Dimension(2, 2);
}

Point::~Point()
{

}

void Point::render2(SDL_Surface *surface)
{
    SDL_FillRect(surface, NULL, 0xFFFFFF);
}

void Point::collide(PhysicalEntity *entity)
{
    double distance = position.distance(entity->getPosition());
    if(distance < 10)
    {
        Dimension unit = (entity->getPosition() - position).unit();
        double magnitude = (10 - distance) * (mass * entity->getMass()) * 0.5 * 0.01;
        entity->applyForce(unit * magnitude);
        applyForce(unit * -magnitude);
    }
}

class PhysicalEnvironment: public Entity
{
    public:
        PhysicalEnvironment();
        ~PhysicalEnvironment();

    protected:
        void update2();
        void render2(SDL_Surface *surface);

        void applyGlobalForces(PhysicalEntity *entity);
        void applyGlobalForce(Dimension force);
        void applyRadialForce(Dimension force, Dimension position, double radius, bool attenuates);
        void applyRadialForce(double force, Dimension position, double radius, bool attenuates);

        void onMouseDown(unsigned short button, Dimension position);
        void onDrag(unsigned short button, Dimension position, Dimension motion);

        double gravity;
        double airResistance;
        double wallElasticity;

        int margin;
};

PhysicalEnvironment::PhysicalEnvironment()
{
    size = Dimension(WIDTH, HEIGHT);

    gravity = 0.01;
    airResistance = 0.005;
    wallElasticity = 0.2;

    margin = 5;
}

PhysicalEnvironment::~PhysicalEnvironment()
{

}

void PhysicalEnvironment::update2()
{
    if(mouse && button == 3)
    {
        applyRadialForce(50, mousePosition, 100, true);
    }

    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        PhysicalEntity *entity = (PhysicalEntity *)(*it);
        applyGlobalForces(entity);

        vector<Entity *>::iterator it2;
        for(it2 = it + 1; it2 < contents.end(); it2++)
        {
            PhysicalEntity *entity2 = (PhysicalEntity *)(*it2);
            entity->collide(entity2);
        }
    }
}

void PhysicalEnvironment::render2(SDL_Surface *surface)
{

}

void PhysicalEnvironment::applyGlobalForces(PhysicalEntity *entity)
{
    entity->applyForce(Dimension(0, gravity * entity->getMass()));
    entity->applyForce(entity->getVelocity() * -airResistance);

    if(entity->getPosition().x < margin)
    {
        entity->applyForce(Dimension((margin - entity->getPosition().x) * wallElasticity, 0));
    }
    else if(entity->getPosition().y < margin)
    {
        entity->applyForce(Dimension(0, (margin - entity->getPosition().y) * wallElasticity));
    }
    else if(entity->getPosition().x >= size.x - margin)
    {
        entity->applyForce(Dimension((size.x - margin - entity->getPosition().x) * wallElasticity, 0));
    }
    else if(entity->getPosition().y >= size.y - margin)
    {
        entity->applyForce(Dimension(0, (size.y - margin - entity->getPosition().y) * wallElasticity));
    }
}

void PhysicalEnvironment::applyGlobalForce(Dimension force)
{
    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        PhysicalEntity *entity = (PhysicalEntity *)(*it);
        entity->applyForce(force);
    }
}

void PhysicalEnvironment::applyRadialForce(Dimension force, Dimension position, double radius, bool attenuates)
{
    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        PhysicalEntity *entity = (PhysicalEntity *)(*it);
        double distance = position.distance(entity->getPosition());
        if(distance <= radius)
        {
            if(attenuates)
            {
                force = force / pow(distance, 2);
            }
            entity->applyForce(force);
        }
    }
}

void PhysicalEnvironment::applyRadialForce(double force, Dimension position, double radius, bool attenuates)
{
    vector<Entity *>::iterator it;
    for(it = contents.begin(); it < contents.end(); it++)
    {
        PhysicalEntity *entity = (PhysicalEntity *)(*it);
        double distance = position.distance(entity->getPosition());
        if(distance <= radius)
        {
            if(attenuates)
            {
                force = force / pow(distance, 2);
            }
            entity->applyForce((entity->getPosition() - position).unit() * force);
        }
    }
}

void PhysicalEnvironment::onMouseDown(unsigned short button, Dimension position)
{
    if(button == 1)
    {
        addEntity(new Point(position));
    }
}

void PhysicalEnvironment::onDrag(unsigned short button, Dimension position, Dimension motion)
{
    if(button == 1)
    {
        addEntity(new Point(position));
    }
}

class Engine
{
    public:
        static Engine *engine;

        Engine();
        ~Engine();

        void start();
        void stop();

        void setContent(Entity *content);

    private:
        bool running;

        long tick;
        long frame;

        Entity *content;

        SDL_Surface *surface;

        void events();
        void update();
        void render();
};

Engine *Engine::engine;

Engine::Engine()
{
    engine = this;

    running = false;

    tick = 0;
    frame = 0;

    content = NULL;

    SDL_Init(SDL_INIT_EVERYTHING);
    SDL_WM_SetCaption("Game Engine", 0);
    surface = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_HWSURFACE | SDL_DOUBLEBUF);
}

Engine::~Engine()
{
    stop();
    SDL_Quit();
}

void Engine::start()
{
    if(!running)
    {
        running = true;
        while(running)
        {
            int now = SDL_GetTicks();

            events();
            for(int i = 0; i < SPF; i++)
            {
                update();
                tick++;
            }
            render();
            frame++;

            int passed = SDL_GetTicks() - now;
            int wait = 1000 / FPS;
            int left = wait - passed;

            SDL_Delay(max(left, 0));
        }
    }
}

void Engine::stop()
{
    running = false;
}

void Engine::setContent(Entity *content)
{
    this->content = content;
}

void Engine::events()
{
    if(content != NULL)
    {
        SDL_Event event;
        while(SDL_PollEvent(&event))
        {
            switch(event.type)
            {
                case SDL_KEYDOWN:
                    content->keyDown(event.key.keysym);
                    break;

                case SDL_KEYUP:
                    content->keyUp(event.key.keysym);
                    break;

                case SDL_MOUSEBUTTONDOWN:
                    content->mouseDown(event.button.button, Dimension(event.button.x, event.button.y));
                    break;

                case SDL_MOUSEBUTTONUP:
                    content->mouseUp(event.button.button, Dimension(event.button.x, event.button.y));
                    break;

                case SDL_MOUSEMOTION:
                    content->mouseMotion(Dimension(event.motion.x, event.motion.y), Dimension(event.motion.xrel, event.motion.yrel));
                    break;

                case SDL_QUIT:
                    stop();
                    break;
            }
        }
    }
}

void Engine::update()
{
    if(content != NULL)
    {
        content->update();
    }
}

void Engine::render()
{
    if(content != NULL)
    {
        content->render(surface);
        SDL_Flip(surface);
    }
}

int main()
{
    Engine engine;

    engine.setContent(new PhysicalEnvironment());
    engine.start();

    return 0;
}