flockanimatordefault.cpp

#include "debug.h"
#include "flock.h"
#include "boid.h"
#include "flockanimatordefault.h"

using namespace irr;
using namespace core;
using namespace scene;

extern Debug* dbg;

FlockAnimatorDefault::FlockAnimatorDefault(IrrlichtDevice* d)
{
    // ctor
    device = d;
    dbg->log("create FlockAnimatorDefault");
}

FlockAnimatorDefault::~FlockAnimatorDefault()
{
    // dtor
    dbg->log("FlockAnimatorDefault deleted");
}

void FlockAnimatorDefault::testSpeedUp(ISceneNode* node)
{
    // testing algo
    // bring all three iterations together to only one

    Flock* flock = ((Flock*) node);
    s32 maxDistance = flock->getNeighborRadius();
    vector3df seekTarget = flock->getTarget();
    s32 cosMaxAngle = 50;
    s32 neighbors = 0;
    vector3df steeringSeparation;
    s32 separationWeight = flock->getSeparationWeight();
    vector3df steeringAlignment;
    s32 alignmnetWeight = flock->getAlignmentWeight();
    vector3df steeringCohesion;
    s32 cohesionWeight = flock->getCohesionWeight();
    vector3df steeringSeekTarget;
    s32 seekTargetWeight = flock->getSeekTargetWeight();

    // for each Boid in Flock
    list<Boid*>::Iterator it = flock->getBoidList().begin();
    for (; it != flock->getBoidList().end(); ++it)
    {
        steeringSeparation.set(0.0f,0.0f,0.0f);
        steeringAlignment.set(0.0f,0.0f,0.0f);
        steeringCohesion.set(0.0f,0.0f,0.0f);
        steeringSeekTarget.set(0.0f,0.0f,0.0f);
        neighbors = 0;

        // for each Boid in Flock
        list<Boid*>::Iterator it2 = flock->getBoidList().begin();
        for (; it2 != flock->getBoidList().end(); ++it2)
        {
            if ((*it)->inBoidNeighborhood((*it2), maxDistance, maxDistance, cosMaxAngle))
            {
                // separation
                vector3df offset = (*it2)->position - (*it)->position;
                f32 distanceSquared = offset.dotProduct(offset);
                steeringSeparation += (offset / -distanceSquared);
                // alignmnet
                steeringAlignment += (*it2)->forward;
                // cohesion
                steeringCohesion += (*it2)->position;

                neighbors++;
            }
        }

        if (neighbors > 0)
        {
            // separation
            steeringSeparation = (steeringSeparation / (f32) neighbors).normalize();
            // alignmnet
            steeringAlignment = ((steeringAlignment / (f32) neighbors) - (*it)->forward).normalize();
            // cohesion
            steeringCohesion = ((steeringCohesion / (f32) neighbors) - (*it)->position).normalize();
        }

        // seek target
        vector3df desiredVelocity = seekTarget - (*it)->position;
        steeringSeekTarget = (desiredVelocity - (*it)->velocity);

        // multiply steering with its weights and set Boid velocity
        steeringSeparation *= separationWeight;
        (*it)->velocity += steeringSeparation;
        steeringAlignment *= ((f32) alignmnetWeight) / 100.0f;
        (*it)->velocity += steeringAlignment;
        steeringCohesion *= cohesionWeight;
        (*it)->velocity += steeringCohesion;
        steeringSeekTarget *= ((f32) seekTargetWeight) / 1000.0f;
        (*it)->velocity += steeringSeekTarget;
    }
}

void FlockAnimatorDefault::animateNode(ISceneNode* node, u32 timeMs)
{
    // debug
    testSpeedUp(node);
    return;

    // old code down to end of this file !!!!!!!!!!!!!!!!!

    // for each Boid in Flock
    Flock* f = ((Flock*) node);
    list<Boid*>::Iterator it = f->getBoidList().begin();
    for (; it != f->getBoidList().end(); ++it)
    {
        // apply all three default rules:
        s32 radius = f->getNeighborRadius();

        vector3df sep = steerForSeparation((*it), f, radius, 50);
        sep *= f->getSeparationWeight();
        (*it)->velocity += sep;

        vector3df alg = steerForAlignment((*it), f, radius, 50);
        alg *= (f32) ((f32) f->getAlignmentWeight()) / 100.0f;
        (*it)->velocity += alg;

        vector3df coh = steerForCohesion((*it), f, radius, 50);
        coh *= f->getCohesionWeight();
        (*it)->velocity += coh;

        // apply addition seekness
        vector3df sek = steerForSeek((*it), f);
        sek *= (f32) ((f32) f->getSeekTargetWeight()) / 1000.0f;
        (*it)->velocity += sek;
    }
}

vector3df FlockAnimatorDefault::steerForSeparation(Boid* boid, Flock* flock, f32 maxDistance, f32 cosMaxAngle)
{
    // steering accumulator and count of neighbors, both initially zero
    vector3df steering;
    s32 neighbors = 0;

    // for each Boid in Flock
    list<Boid*>::Iterator it = flock->getBoidList().begin();
    for (; it != flock->getBoidList().end(); ++it)
    {
        if (boid->inBoidNeighborhood((*it), flock->getNeighborRadius() , maxDistance, cosMaxAngle))
        {
            // add in steering contribution
            // (opposite of the offset direction, divided once by distance
            // to normalize, divided another time to get 1/d falloff)
            vector3df offset = (*it)->position - boid->position;
            f32 distanceSquared = offset.dotProduct(offset);
            steering += (offset / -distanceSquared);

            // count neighbors
            neighbors++;
        }
    }
    // divide by neighbors, then normalize to pure direction
    if (neighbors > 0)
    {
        steering = (steering / (f32) neighbors).normalize();
    }

    //boid->addDebugText("neighbors", neighbors);

    return steering;
}

vector3df FlockAnimatorDefault::steerForAlignment(Boid* boid, Flock* flock, f32 maxDistance, f32 cosMaxAngle)
{
    // steering accumulator and count of neighbors, both initially zero
    vector3df steering;
    s32 neighbors = 0;

    // for each Boid in Flock
    list<Boid*>::Iterator it = flock->getBoidList().begin();
    for (; it != flock->getBoidList().end(); ++it)
    {
        if (boid->inBoidNeighborhood((*it), flock->getNeighborRadius(), maxDistance, cosMaxAngle))
        {
            // accumulate sum of neighbor's heading
            steering += (*it)->forward;

            // count neighbors
            neighbors++;
        }
    }

    // divide by neighbors, subtract off current heading to get error-
    // correcting direction, then normalize to pure direction
    if (neighbors > 0)
    {
        steering = ((steering / (f32) neighbors) - boid->forward).normalize();
    }

    return steering;
}

vector3df FlockAnimatorDefault::steerForCohesion(Boid* boid, Flock* flock, f32 maxDistance, f32 cosMaxAngle)
{
    // steering accumulator and count of neighbors, both initially zero
    vector3df steering;
    s32 neighbors = 0;

    // for each Boid in Flock
    list<Boid*>::Iterator it = flock->getBoidList().begin();
    for (; it != flock->getBoidList().end(); ++it)
    {
        if (boid->inBoidNeighborhood((*it), flock->getNeighborRadius(), maxDistance, cosMaxAngle))
        {
            // accumulate sum of neighbor's positions
            steering += (*it)->position;

            // count neighbors
            neighbors++;
        }
    }

    // divide by neighbors, subtract off current position to get error-
    // correcting direction, then normalize to pure direction
    if (neighbors > 0)
    {
        steering = ((steering / (f32) neighbors) - boid->position).normalize();
    }

    return steering;
}

vector3df FlockAnimatorDefault::steerForSeek(Boid* boid, Flock* flock)
{
    vector3df desiredVelocity = flock->getTarget() - boid->position;
    return (desiredVelocity - boid->velocity);
}

---