decision.cpp

RoboCup 3D 仿真组清华大学2005的源代码

/***************************************************************************
 *   Copyright (C) 2005 by Rujia Liu                                       *
 *   [e-mail]                                                  *
 *                                                                         *
 *   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 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <soccer/soccertypes.h>
#include "decision.h"
#include "agent.h"
#include "worldmodel.h"
#include "skill.h"
#include "formation.h"
#include "situation.h"
#include "geometry.h"

extern Agent agent;
extern WorldModel wm;
extern Skill skill;
extern Formation formation;
extern Situation situation;

extern AgentParam param;

using namespace salt;
using namespace Geometry;

// state matchine
#define STATUS_NONE 0

Decision::Decision()
{
}

Decision::~Decision()
{
}

void Decision::Think()
{
    // 7 two-side playmodes
    TTeamIndex ti = wm.GetTeamIndex();
    TPlayMode myKickOff = (wm.GetTeamIndex() == TI_LEFT) ? PM_KickOff_Left : PM_KickOff_Right;
    TPlayMode myKickIn = (wm.GetTeamIndex() == TI_LEFT) ? PM_KickIn_Left : PM_KickIn_Right;
    TPlayMode myCornerKick = (wm.GetTeamIndex() == TI_LEFT) ? PM_CORNER_KICK_LEFT : PM_CORNER_KICK_RIGHT;
    TPlayMode myGoalKick = (wm.GetTeamIndex() == TI_LEFT) ? PM_GOAL_KICK_LEFT : PM_GOAL_KICK_RIGHT;
    TPlayMode myOffside = (wm.GetTeamIndex() == TI_LEFT) ? PM_OFFSIDE_LEFT : PM_OFFSIDE_RIGHT;
    TPlayMode myGoal = (wm.GetTeamIndex() == TI_LEFT) ? PM_Goal_Left : PM_Goal_Right;
    TPlayMode myFreeKick = (wm.GetTeamIndex() == TI_LEFT) ? PM_FREE_KICK_LEFT : PM_FREE_KICK_RIGHT;   

    // call it to gather information
    situation.CycleStart();    
    switch(wm.GetPlayMode())
    {
        case PM_BeforeKickOff :
            BehaveBeforeKickOff();
            break;
        case PM_KickOff_Left :
        case PM_KickOff_Right :
            if (wm.GetPlayMode() == myKickOff) BehaveMyKickOff(); else BehaveTheirKickOff();
            break;
        case PM_PlayOn :  
            BehavePlayOn();
            break;
        case PM_KickIn_Left :
        case PM_KickIn_Right :
            if (wm.GetPlayMode() == myKickIn) BehaveMyKickIn(); else BehaveTheirKickIn();
            break;
        case PM_CORNER_KICK_LEFT:
        case PM_CORNER_KICK_RIGHT:
            if (wm.GetPlayMode() == myCornerKick) BehaveMyCornerKick(); else BehaveTheirCornerKick();
            break;
        case PM_GOAL_KICK_LEFT:
        case PM_GOAL_KICK_RIGHT:
            if (wm.GetPlayMode() == myGoalKick) BehaveMyGoalKick(); else BehaveTheirGoalKick();
            break;
        case PM_OFFSIDE_LEFT:
        case PM_OFFSIDE_RIGHT:
            if (wm.GetPlayMode() == myOffside) BehaveMyOffside(); else BehaveTheirOffside();
            break;
        case PM_GameOver:
            BehaveGameOver();
            break;
        case PM_Goal_Left:
        case PM_Goal_Right:
            if (wm.GetPlayMode() == myGoal) BehaveMyGoal(); else BehaveTheirGoal();
            break;
        case PM_FREE_KICK_LEFT:
        case PM_FREE_KICK_RIGHT:
            if (wm.GetPlayMode() == myFreeKick) BehaveMyFreeKick(); else BehaveTheirFreeKick();
            break;
        default:
            break;          
        }
}

void Decision::BehaveBeforeKickOff()
{
    Vector3f pos = formation.GetInitialFormation(wm.GetTeamUnum()); 
    agent.Beam(Vector3f(pos));
    agent.Drive(Vector3f(0,0,0));
}

void Decision::BehaveMyKickOff()
{
    BehavePlayOn();
}

void Decision::BehaveTheirKickOff()
{
    Vector3f pos = formation.GetInitialFormation(wm.GetTeamUnum()); 
    skill.Goto(pos);
}

void Decision::BehavePlayOn()
{
  // Goalie
  status = STATUS_NONE;
  if(formation.IsGoalie(wm.GetTeamUnum()))
  {   
      // do not intercept a ball outside 
      if(situation.InField(wm.GetBallPosition()) && skill.Interceptable())
      {
          log.Log("(Goalie) trying to intercept\n");
          skill.RunToKick();
      }
      else
      {
          Vector3f ballpos = wm.GetBallPosition();
          Vector3f pos;
          pos[0] = -wm.GetFieldLength() / 2.0;
          pos[1] = 0.0;
  
          // see which enemy is about to kick the ball
          int i;
          float shoottarget;
          for(i = 1; i <= 11; i++)
              if(situation.OkToShoot(1, i, shoottarget))
              {
                  log.Log("(Goalie) blocking at pos[1] = %.3f\n", shoottarget);
                  pos[1] = shoottarget;
              }
      
          if(pos[1] < -wm.GetGoalWidth()) pos[1] = -wm.GetGoalWidth();
          if(pos[1] > wm.GetGoalWidth()) pos[1] = wm.GetGoalWidth();	  
          log.Log("(Goalie) target pos = (%.3f, %.3f)\n", pos[0], pos[1]);
  
          // goto the defence point!
          skill.Goto(pos);
      }  
  }
  else
  {
      if(!skill.Interceptable())
      {
          log.Log("(Decision) Positioning.\n");
          Position();
      }
      else
      {
          Vector3f ballpos = wm.GetBallPosition();
          Vector3f mypos = wm.GetMyPosition();
  
          // handle ball soon
          if((mypos-ballpos).Length() < 1.0)
          { 
              // the distance could be changed according to oppmodel
              
              // in our penalty area!!!
              if(situation.InPenaltyArea(0, ballpos))
              {
                  skill.Intercept(-5.0, 5.0, 0.0);
                  if(situation.Dangerous() && ballpos[0] > mypos[0])
                  {
                      log.Log("(Our Penalty Area) Dangerous!\n");
                      agent.Kick(50, 100);
                  }
                  else if(ballpos[0] - mypos[0] > 0.3)
                  {
                      log.Log("(Our Penalty Area) Normal kick\n");
                      agent.Kick(30, 100);
                  }
                  else
                  {
                      log.Log("(Our Penalty Area) I don't want to, but I have to\n");
                      agent.Kick(0, 1);
                  }
              }
              else // normal play: see if pass is possible, then consider dribble
              {
                  float dist;
                  float angle, power;
                  float pass_priority = GetPassPriority(dist);
                  float dribble_priority = GetDribblePriority(angle);
                  float shoot_priority = GetShootPriority();
                  if(shoot_priority > 0.5) // shoot whenever possible
                  {
                      log.Log("(Decision) Want to shoot!\n");
                      skill.Shoot();
                  }
                  else if(pass_priority > 0.5) // pass whenever possible
                  {
                      log.Log("(Decision) Want to pass! dist = %.3f\n", dist);
                      skill.Pass(dist);
                  }
                  else
                  {
                      log.Log("(Decision) Want to dribble! angle = %.3f\n", angle);
                      skill.Dribble(angle);
                  }
              }
          }
          else
          {
              float angle = GetPassAngle();
              log.Log("(Decision) Intercepting at pass angle %.3f\n", angle);
              int unum = GetBestPassTeammate();
              log.Log("(Decision) Actually, I wanna pass to %d\n", unum);
              skill.Intercept(angle - 5.0, angle + 5.0, 0.0);
          }         
      }
  }   
}

#define STATUS_MYKICKIN_GOTO  1
#define STATUS_MYKICKIN_KICK  2
void Decision::BehaveMyKickIn()
{
    if(skill.Interceptable())
    {
        if(status == STATUS_NONE) status = STATUS_MYKICKIN_GOTO;

        Vector3f mypos = wm.GetMyPosition();
        Vector3f target = wm.GetBallPosition();
        mypos[2] = target[2] = 0.0;
        target[0] -= 1.0f;
        if(target[1] > 0) target[1] += 0.2f;
        else target[1] -= 0.2f;

        switch(status)
        {
            case STATUS_MYKICKIN_GOTO:
                log.Log("(MyKickIn) STATUS_MYKICKIN_GOTO\n");
                skill.Goto(target);
                if((mypos-target).Length() < 0.1)
                    status = STATUS_MYKICKIN_KICK;
                break;
            case STATUS_MYKICKIN_KICK:
                log.Log("(MyKickIn) STATUS_MYKICKIN_KICK\n");
                agent.Drive(Vector3f(100.0, 0.0, 0.0));
                agent.Kick(0, 1); // just like hit
                break;
        }
    }
    else
        BehavePlayOn();
}

// TODO: mark the opponents!
void Decision::BehaveTheirKickIn()
{
    BehavePlayOn();
}

#define STATUS_MYCORNERKICK_GOTO1  1
#define STATUS_MYCORNERKICK_GOTO2  2
#define STATUS_MYCORNERKICK_KICK   3
void Decision::BehaveMyCornerKick()
{
    if(skill.Interceptable())
    {
        if(status == STATUS_NONE) status = STATUS_MYCORNERKICK_GOTO1;

        Vector3f mypos = wm.GetMyPosition();
        Vector3f target = wm.GetBallPosition();
        mypos[2] = target[2] = 0.0;
        // go beside, then behind, then accelerate with a powerful kick towards the goal!
        switch(status)
        {
            case STATUS_MYCORNERKICK_GOTO1:
                log.Log("(Decision) STATUS_MYCORNERKICK_GOTO1\n");
                target[0] += 1.0f; // kick back
                if(target[1] > 0) 
                    target[1] += 0.2f;
                else 
                    target[1] -= 0.2f;
                skill.Goto(target);
                if((mypos-target).Length() < 0.1)
                status = STATUS_MYCORNERKICK_GOTO2;
                break;
            case STATUS_MYCORNERKICK_GOTO2:
                log.Log("(Decision) STATUS_MYCORNERKICK_GOTO2\n");
                target[0] += 0.08f; // kick back
                if(target[1] > 0)
                    target[1] += 1.0f;
                else 
                    target[1] -= 1.0f;
                skill.Goto(target);
                if((mypos-target).Length() < 0.05)
                    status = STATUS_MYCORNERKICK_KICK;
                break;
            case STATUS_MYCORNERKICK_KICK:
                log.Log("(Decision) STATUS_MYCORNERKICK_KICK\n");
                if(target[1] > 0)
                    agent.Drive(Vector3f(0.0, -100.0, 0.0));
                else
                    agent.Drive(Vector3f(0.0, 100.0, 0.0));
                agent.Kick(0, 100); // powerful kick!
                break;
        }
    }
    else
        BehavePlayOn();
}

// TODO: mark and protect the goal
void Decision::BehaveTheirCornerKick()
{
    BehavePlayOn();
}

#define STATUS_MYGOALKICK_GOINGBACK     1
#define STATUS_MYGOALKICK_GOINGFORWARD  2

void Decision::BehaveMyGoalKick()
{
      BehavePlayOn();
}

void Decision::BehaveTheirGoalKick()
{
    BehavePlayOn();
}

void Decision::BehaveMyOffside()
{
    BehavePlayOn();
}

void Decision::BehaveTheirOffside()
{
    BehavePlayOn();
}

void Decision::BehaveGameOver()
{
    agent.Drive(Vector3f(0,0,0));
}

void Decision::BehaveMyGoal()
{
    Vector3f pos = formation.GetInitialFormation(wm.GetTeamUnum()); 
    skill.Goto(pos);
}

void Decision::BehaveTheirGoal()
{
    Vector3f pos = formation.GetInitialFormation(wm.GetTeamUnum()); 
    skill.Goto(pos);
}