Basic Boid

#include <nds.h>
#include <vector>

#include "Application.h"
#include "Vector2D.h"

#include "Boid.h"
#include "Graphics.h"

using namespace std;

Boid::Boid(Application* argOwner, int argX, int argY) { //
    // Reference to the owning application:
    owner   = argOwner;

    // Physics related vectors:
    lastPosition    = new Vector2D(argX, argY);
    position        = new Vector2D(argX, argY);
    velocity        = new Vector2D(1, 0);

    setColor(RGB15(31,31,31) | (1<<15));
}


void Boid::update(void) {
    // Store the current position so we can use verlet integration lateron:
    lastPosition->x = position->x;
    lastPosition->y = position->y;

    // Basic motion:
    position->x += velocity->x;
    position->y += velocity->y;

    // Flip the velocity if the screenbounds are reached (yet to impletement resolveLocation)
    if(position->x >= SCREEN_WIDTH  || position->x <= 0) velocity->x *= -1;
    if(position->y >= SCREEN_HEIGHT || position->y <= 0) velocity->y *= -1;
}

void Boid::draw(Graphics* g) {
    unsigned int collisionCount = 0;
    Vector2D force;

    // Get all the boids in this application:
    vector<Boid*>* allBoids = owner->getBoids();

    for(unsigned int i = 0; i < allBoids->size(); i++) {
        if(this != allBoids->at(i)) {
            if(allBoids->at(i)->getPosition()->distanceTo(position) < 50) {
                collisionCount++;
                force.x += allBoids->at(i)->getVelocity()->x;
                force.y += allBoids->at(i)->getVelocity()->y;
            }
        }
    }

    if(collisionCount > 0) {
        force.x *= 1 / collisionCount;
        force.y *= 1 / collisionCount;

        if(force.getLengthSQ() > 1) {
            force.normalize();

            force.x *= 1;
            force.y *= 1;
        }

        velocity->x += force.x;
        velocity->y += force.y;

        setColor(RGB15(31,0,0) | (1<<15));
    } else {
        printf("\x1b[%i;0H %f ", 3, 0.0);
        setColor(RGB15(31,31,31) | (1<<15));
    }

    // Erase the old pixel (draw a black one on top)
    //g->setColor(RGB15(0,0,0) | (1<<15));
    //g->drawPixel(lastPosition->x, lastPosition->y);

    // Draw the new pixel!
    g->setColor(color);
    g->drawPixel(position->x, position->y);
}