source: orxonox.OLD/branches/blink/src/ai/swarm_gorel.cc @ 10652

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Thomas Fahrni
   co-programmer:
*/
#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_AI
#include "swarm_gorel.h"
#include "ai_module.h"
#include "movement_module.h"

void SwarmGoRel::initialize()
{
        std::map<WorldEntity*,AIModule*>::iterator it;

        //calculate  movement parameters..
        float averageRadius;
        for (it=members.begin(); it!= members.end(); it++ ){
                averageRadius+=it->second->getNPCRadius();
        }
        averageRadius=averageRadius/members.size();
        aMax=80;//400/averageRadius;
        vMax=maxSpeed;//1000/averageRadius;//300/averageRadius;
        viewChangeMax=90/averageRadius;

        //load correct ai-module..
        for (it= members.begin(); it!= members.end(); it++ ){
                changeAIModule(it, new MovementModule());
        }

        //get swarm position..
        //position=this->getPosition();
}

void SwarmGoRel::process(float dt)
{
        std::map<WorldEntity*,AIModule*>::iterator it;

        //Vector swarmPosition=this->getPosition();
        Vector correction=Vector(0,0,0);
        Vector destination;

        if(taskRelObject!=NULL && taskMaxTime>0){
                destination=taskRelObject->getAbsCoor()+taskRelPos;
                correction=(destination-position)-view*speed;
                correction.y=0;
                taskMaxTime-=dt;
        }else{
                taskComplete=true;
        }

        float correctionLen=correction.len();
        if(correctionLen>aMax*dt)correction=correction/correctionLen*aMax*dt;

        //std::cout << angleRad(correction,movement) << "\n";
        Vector movement=view*speed+correction;

        float movementLen=movement.len();
        if(movementLen>vMax)movement=movement/movementLen*vMax;

        float newSpeed=movement.len();
        Vector newView=movement.getNormalized();

        /*if((newSpeed<=taskSpeed && speed>=taskSpeed)||(newSpeed>=taskSpeed && speed<=taskSpeed)){
                newSpeed=taskSpeed;
        }else if((newSpeed>speed && speed<taskSpeed) || (newSpeed < speed && speed < taskSpeed)){
                newSpeed=taskSpeed;
        }*/

        /*if(angleDeg(view,newView)>viewChangeMax){
                std::cout << "alarm\n";
        }*/
        if(newSpeed<40)newSpeed=40;
        if(newSpeed>vMax)newSpeed=vMax;

        speed=newSpeed;
        view=newView;
        position=position+view*speed*dt;

        //tell orders to swarm-members...
        for (it= members.begin(); it!= members.end(); it++ ){
                it->second->setDestination(position);
                it->second->setDestinationMovement(view*speed);
                it->second->process(dt);
        }

        //check if destination reached
        if(!taskComplete){
                //swarmPosition=this->getPosition();
                if((destination-position).len()<10)taskComplete=true;
        }
}