selfpass_bms.c

Brainstormers(头脑风暴)队是05年robocup冠军,这是05年Brainstormers公布的源代码,Brainstormers是robocup老牌的德国强队

/*
Brainstormers 2D (Soccer Simulation League 2D)
PUBLIC SOURCE CODE RELEASE 2005
Copyright (C) 1998-2005 Neuroinformatics Group,
                        University of Osnabrueck, Germany

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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "selfpass_bms.h"
#include "ws_memory.h"
#include "tools.h"

bool Selfpass::initialized= false;

#if 0
#define DBLOG_MOV(LLL,XXX) LOG_POL(LLL,<<"Selfpass: "<<XXX)
//#define DBLOG_DRAW(LLL,XXX) LOG_POL(LLL,<<_2D<<XXX)
#define DBLOG_DRAW(LLL,XXX)
#else
#define DBLOG_MOV(LLL,XXX)
#define DBLOG_DRAW(LLL,XXX)
#endif

/*************************************************************************/

/* Initialization */

bool Selfpass::init(char const * conf_file, int argc, char const* const* argv) {
  if(initialized) return true; // only initialize once...
  initialized= true;
  cout<<"\nSelfpass behavior initialized.";
  return (BasicCmd::init(conf_file, argc, argv)
	  && OneOrTwoStepKick::init(conf_file,argc,argv)
	  );
}

Selfpass::Selfpass(){
  ValueParser vp(CommandLineOptions::policy_conf,"Selfpass_bms");
  //vp.set_verbose(true);
  basic_cmd = new BasicCmd;
  onetwostepkick = new OneOrTwoStepKick;
}

Selfpass::~Selfpass() {
  delete basic_cmd;
}

bool Selfpass::get_cmd(Cmd & cmd){
  return false;
}

bool Selfpass::get_cmd(Cmd & cmd, const ANGLE targetdir, const Value speed, const Vector target){
WSpset opp=WSinfo::alive_opponents;
opp.keep_and_sort_closest_players_to_point(1,WSinfo::ball->pos);
return get_cmd(WSinfo::me->pos,WSinfo::me->vel,WSinfo::me->ang,WSinfo::ball->pos,WSinfo::ball->vel,
                           opp[0]->pos,opp[0]->ang,cmd,targetdir,speed,target);
}

bool Selfpass::get_cmd(Vector my_pos,Vector my_vel,ANGLE my_ang,Vector ball_pos,Vector ball_vel,
Vector opp_pos,ANGLE opp_ang,Cmd & cmd, const ANGLE targetdir, const Value speed, const Vector target){
  Value speed1, speed2;

	Vector new_my_pos,new_my_vel,new_ball_pos,new_ball_vel;
	ANGLE new_my_ang;

  if(speed >0){ // first command is a kick
    LOG_MOV(0," Selfpass: get cmd: 12stepkick "
	      <<" speed  "<<speed<<" to target "<<target);
    onetwostepkick->set_state(my_pos,my_vel,my_ang,ball_pos,ball_vel,
                                               opp_pos,opp_ang,0);
    onetwostepkick->kick_to_pos_with_initial_vel(speed,target);
    onetwostepkick->get_vel(speed1,speed2);
    if(fabs(speed1-speed)>0.1){
      LOG_MOV(0,"Get cmd HMM - onestepkick not possible !-> Trying 2 step kick");
    }
    onetwostepkick->get_cmd(cmd);
    return true;
  }
  // begin movement
  if(targetdir.diff(my_ang) >5/180.*PI){ // have to turn
    Value moment = (targetdir-my_ang).get_value_mPI_pPI() *
      (1.0 + (WSinfo::me->inertia_moment * (my_vel.norm())));
    if (moment > 3.14) moment = 3.14;
    if (moment < -3.14) moment = -3.14;
    basic_cmd->set_turn(moment);
    DBLOG_MOV(0," get cmd "
	      <<" targetdir "<<RAD2DEG(targetdir.get_value())
	      <<" tmpdir "<<RAD2DEG(WSinfo::me->ang.get_value())
	      <<" computed moment "<<RAD2DEG(moment)
	      <<": Have to turn by "<<RAD2DEG((targetdir-WSinfo::me->ang).get_value_mPI_pPI()));
    basic_cmd->get_cmd(cmd);
    return true;
  }
  else{ // kicked, turned, now dashing
		// (was: just dash 100)
		Cmd tmpCmd;
		int dash_pow=100;
		static const float minDistToBall = 1.35*(ServerOptions::player_size + ServerOptions::ball_size);
		float distToBall;
		bool isSafe = false;
		while(dash_pow>90){
			tmpCmd.cmd_main.unset_lock();
			basic_cmd->set_dash(dash_pow);
			basic_cmd->get_cmd(tmpCmd);
			Tools::model_cmd_main(WSinfo::me->pos,WSinfo::me->vel,WSinfo::me->ang,WSinfo::ball->pos,WSinfo::ball->vel,tmpCmd.cmd_main,new_my_pos,new_my_vel,new_my_ang,new_ball_pos,new_ball_vel,false);
			distToBall = (new_my_pos-new_ball_pos).norm();
			if(distToBall>minDistToBall) {
				isSafe = true;
				break;
			}
			dash_pow-=10;
		}
		if(!isSafe)
			return false;
    basic_cmd->set_dash(dash_pow);
    basic_cmd->get_cmd(cmd);
    return true;
  }
  return false;
}

void Selfpass::determine_optime2react(const ANGLE targetdir, const int max_dashes){
#define MAX_STEPS 10

  Cmd tmp_cmd;
  Vector desired_ballpos_rel2me;
  desired_ballpos_rel2me.init_polar (0.7,targetdir.get_value()); // that's where the ball finally should be
  Vector tmp_pos = WSinfo::me->pos + desired_ballpos_rel2me;
  Vector next_pos, next_vel, next_ballpos, next_ballvel;
  Vector tmp_vel = WSinfo::me->vel;
  Vector tmp_ballvel = WSinfo::ball->vel;
  Vector tmp_ballpos = WSinfo::ball->pos;
  ANGLE tmp_ang = WSinfo::me->ang;
  Angle next_ang;
  const int cycles2kick = 1; // assumption: only one cycle is needed to kick.
  Value opradius;
  int tmp_stamina = (int) WSinfo::me->stamina;
  int next_stamina;
  const int op_time2react = 0;

  int i=0;
  for (int idx = 0; idx <50; idx++) 
    optime2react[idx] = -1; // init

  while(i<max_dashes +3){
    //    DBLOG_MOV(0,"determine ops: step "<<i);
    // simulate player: 
    tmp_cmd.cmd_main.unset_lock();
    tmp_cmd.cmd_main.unset_cmd();
      
    if(i<cycles2kick){ // do kicks
      basic_cmd->set_turn(0);
      // do nothing, but simulate player movement (velocity)
      tmp_ballvel = WSinfo::ball->vel; // virtually hold the ball to test what happens, if ball just
      tmp_ballpos = WSinfo::ball->pos; // continues without being kicked
    }
    else if(targetdir.diff(tmp_ang) >5/180.*PI){ // have to turn
      Value moment = (targetdir-tmp_ang).get_value_mPI_pPI() * 
	(1.0 + (WSinfo::me->inertia_moment * (tmp_vel.norm())));
      if (moment > 3.14) moment = 3.14;
      if (moment < -3.14) moment = -3.14;
      basic_cmd->set_turn(moment);
      basic_cmd->get_cmd(tmp_cmd);
    }
    else{ // kicked, turned, now dashing
      int dash_power = 100;
      if(tmp_stamina <= ServerOptions::recover_dec_thr*ServerOptions::stamina_max + 100.){
	dash_power = (int)WSinfo::me->stamina_inc_max;
	DBLOG_MOV(2,"Reducing Stamina to "<<dash_power);
      }
      DBLOG_MOV(2,"step "<<i<<": Dash "<<dash_power);
      basic_cmd->set_dash(dash_power);
      basic_cmd->get_cmd(tmp_cmd);	
    }
      
    Tools::model_cmd_main(tmp_pos,tmp_vel,tmp_ang.get_value(),tmp_stamina, tmp_ballpos, tmp_ballvel,
			  tmp_cmd.cmd_main, 
			  next_pos,next_vel,next_ang,next_stamina, next_ballpos, next_ballvel);
    DBLOG_MOV(2,"step "<<i<<" new position: "<<next_pos<<" new angle "<<RAD2DEG(next_ang)
	      <<" new stamina "<<next_stamina);
    DBLOG_DRAW(0,C2D(next_pos.x,next_pos.y,0.1,"red"));
    DBLOG_DRAW(0,C2D(next_ballpos.x,next_ballpos.y,0.1,"grey"));
      
    // check opponents;
    WSpset pset = WSinfo::valid_opponents;

    //DBLOG_MOV(0,"num ops "<<pset.num);
    for (int idx = 0; idx < pset.num; idx++) {
      if(optime2react[idx]>=0)
	continue; // already set
      //DBLOG_MOV(0,"checking idx "<<idx);
      if(pset[idx]->number == WSinfo::ws->his_goalie_number){
	opradius = ServerOptions::catchable_area_l + (i+(1-op_time2react)) * ServerOptions::player_speed_max;
      }
      else
	opradius = pset[idx]->kick_radius + (i+(1-op_time2react)) * ServerOptions::player_speed_max;
      // ridi: Worst Case: max. vel.
      // refinement: if opponent has a bad body dir, compute acceleration of opponent
      //DBLOG_DRAW(0,C2D(pset[idx]->pos.x,pset[idx]->pos.y,opradius,"magenta"));
      if(next_pos.distance(pset[idx]->pos) < opradius){
	DBLOG_DRAW(0,C2D(pset[idx]->pos.x,pset[idx]->pos.y,opradius,"magenta"));
	if(pset[idx]->ang.diff((next_pos-pset[idx]->pos).ARG()) >5./180.*PI){
	  DBLOG_MOV(2,"step "<<i<<" Opponent "<<pset[idx]->number<<" gets me but has to turn");
	  optime2react[idx] = 1; // opponent has to turn
	}
	else{
	  DBLOG_MOV(2,"step "<<i<<" Opponent "<<pset[idx]->number<<" gets me and must not turn");
	  optime2react[idx] = 0; 
	}
	if(WSinfo::his_goalie != NULL){
	  if(pset[idx] == WSinfo::his_goalie)
	    optime2react[idx] = 0; // take special care of goalie!	  
	}
      }  //op gets me
    }  // for all opponents...
    i++;
  } // while i< max_steps

  for (int idx = 0; idx <50; idx++) 
    if(optime2react[idx] == -1)  // reset all those w.no normal time2react
      optime2react[idx] = 0; 
}





bool Selfpass::is_selfpass_safe(const ANGLE targetdir, Value &speed, Vector &ipos, int &steps,
				Vector &attacking_op, int & op_number, const int max_dashes,
				Value op_time2react){

  //op_time2react is the time that is assumed the opponents need to react. 0 is worst case, 1 assumes
  // that they need 1 cycle to react. This is already pretty aggressive and (nearly) safe

#define MAX_STEPS 10
#define OUTOF_PITCH_SAFETY 3
  //#define OUTOF_PITCH_SAFETY 10

  determine_optime2react(targetdir,max_dashes);

  Cmd tmp_cmd;
  Vector desired_ballpos_rel2me;
  desired_ballpos_rel2me.init_polar (0.7,targetdir.get_value()); // that's where the ball finally should be
  Vector safe_ipos = WSinfo::ball->pos;
  Vector tmp_pos = WSinfo::me->pos + desired_ballpos_rel2me;
  Vector next_pos, next_vel, next_ballpos, next_ballvel;
  Vector tmp_vel = WSinfo::me->vel;
  Vector tmp_ballvel = WSinfo::ball->vel;
  Vector tmp_ballpos = WSinfo::ball->pos;
  ANGLE tmp_ang = WSinfo::me->ang;
  Angle next_ang;
  Value decay =1.;
  Value summed_decay = 0;
  const int cycles2kick = 1; // assumption: only one cycle is needed to kick.
  bool op_gets_me = false;
  bool op_gets_ball = false;
  bool outof_pitch = false;
  Value opradius;
  int safe_steps = 0;
  int safe_steps_without_kicking = 0;
  Vector safe_ipos_without_kicking = WSinfo::ball->pos;
  Vector attacker = Vector(52.0,0); // default: op goalie attacks
  Vector attacker_without_kicking = Vector(52.0,0); // default: op goalie attacks
  int number_of_dashes = 0;
  int safe_dashes_with_kick = 0;
  int safe_dashes_without_kick = 0;
  int tmp_stamina = (int) WSinfo::me->stamina;
  int next_stamina;
  int attacker_number = 0;
  int attacker_number_without_kicking = 0;
  Value outof_pitch_safety = OUTOF_PITCH_SAFETY;

  // *  ridi 05: plausibilty check first!
  if(WSinfo::ball->pos.x + outof_pitch_safety> FIELD_BORDER_X){ // if Ball is already close to border of pitch
    if(fabs(targetdir.get_value_mPI_pPI() )<90/180. *PI){
      DBLOG_MOV(0,"Ball close to border. forbidden to proceed !");
      return false;
    }
  }

  if(WSinfo::ball->pos.x - outof_pitch_safety<- FIELD_BORDER_X){ // if Ball is already close to border of pitch
    if(fabs(targetdir.get_value_mPI_pPI() )>90/180. *PI){
      DBLOG_MOV(0,"Ball close to border. forbidden to proceed !");
      return false;
    }
  }

  if(WSinfo::ball->pos.y + outof_pitch_safety> FIELD_BORDER_Y){ // if Ball is already close to border of pitch
    if(targetdir.get_value_mPI_pPI()<0){
      DBLOG_MOV(0,"Ball close to border. forbidden to proceed !");
      return false;
    }
  }

  if(WSinfo::ball->pos.y - outof_pitch_safety<- FIELD_BORDER_Y){ // if Ball is already close to border of pitch
    if(targetdir.get_value_mPI_pPI()>0){
      DBLOG_MOV(0,"Ball close to border. forbidden to proceed !");
      return false;