selfpass2_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 "selfpass2_bms.h"
#include "ws_memory.h"
#include "tools.h"

bool Selfpass2::initialized= false;

#if 1
#define DBLOG_MOV(LLL,XXX) LOG_POL(LLL,<<"Selfpass2: "<<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


#define BODYDIR_TOLERANCE 5.0   // allowed tolerance before turning into target direction
#define MIN_DIST_2_BORDER 2.  // keep away from border
#define SAFETY_MARGIN 0.2  // want to have the ball safe!


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

/* Initialization */

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

Selfpass2::Selfpass2(){
  basic_cmd = new BasicCmd;
  onetwostepkick = new OneOrTwoStepKick;
}

Selfpass2::~Selfpass2() {
  delete basic_cmd;
  delete onetwostepkick;
}

bool Selfpass2::get_cmd(Cmd & cmd){

  DBLOG_MOV(0,"get_cmd: No precomputation available! Call is_safe(targetdir) first!!!");
  return false;
}

/*
bool Selfpass2::get_cmd(Cmd & cmd){

  if(simulation_table[0].valid_at != WSinfo::ws->time){
    DBLOG_MOV(0,"get_cmd: No precomputation available! Call is_safe(targetdir) first!!!");
    return false;
  }
  cmd = simulation_table[0].cmd;
  return true;
}
*/

bool Selfpass2::get_cmd(Cmd & cmd, const ANGLE targetdir, const Vector targetpos, const Value kickspeed){

  DBLOG_MOV(0,"get_cmd: targetdir: "<<RAD2DEG(targetdir.get_value())<<" targetpos "<<targetpos
	    <<" kickspeed "<<kickspeed);

	    
  if(kickspeed >0){ // want to kick -> compute appropriate onestep kick
    Value speed1, speed2;
    onetwostepkick->reset_state(); // use current ws-state
    onetwostepkick->kick_to_pos_with_initial_vel(kickspeed,targetpos);
    onetwostepkick->get_vel(speed1,speed2);
    if(fabs(speed1-kickspeed)<=0.1){
      // everything's fine!
      onetwostepkick->get_cmd(cmd);
      return true;
    }
    DBLOG_MOV(0,"get_cmd: PROBLEM. Should kick, but kick does not work. CALL ridi!!!");
    return false;
  }

  // have to turn or dash to targetpos
  get_cmd_to_go2dir(cmd, targetdir, WSinfo::me->pos, WSinfo::me->vel,WSinfo::me->ang,(int)( WSinfo::me->stamina), WSinfo::me->inertia_moment, WSinfo::me->stamina_inc_max);
  return true;
}



bool Selfpass2::is_selfpass_safe(const ANGLE targetdir){

  Vector ipos,op;
  Value speed;
  int steps, op_num;

  return is_selfpass_safe(targetdir, speed,ipos, steps, op, op_num);
}

bool Selfpass2::is_selfpass_still_safe(const ANGLE targetdir, Value & kickspeed, int &op_num){
  // used to recheck previous intention. checks nokickc only!

  Vector ipos,op;
  int steps;

  return is_selfpass_safe(targetdir, kickspeed,ipos, steps, op, op_num, true);
}

bool Selfpass2::is_turn2dir_safe(const ANGLE targetdir, Value &kickspeed, Vector &targetpos, int &actual_steps,
				 Vector &attacking_op, int & op_number, 
				 const bool check_nokick_only, const int max_dashes){
  return is_turn2dir_safe(targetdir, kickspeed, targetpos, actual_steps, attacking_op, op_number,
			  WSinfo::me->pos, WSinfo::me->vel, WSinfo::me->ang, WSinfo::me->stamina,
			  WSinfo::ball->pos, WSinfo::ball->vel,
			  check_nokick_only, max_dashes);
}

bool Selfpass2::is_turn2dir_safe(const ANGLE targetdir, Value &kickspeed, Vector &targetpos, int &actual_steps,
				 Vector &attacking_op, int & op_number, 
				 const Vector mypos, const Vector myvel, const ANGLE myang,
				 const Value mystamina,
				 const Vector ballpos, const Vector ballvel,
				 const bool check_nokick_only, const int max_dashes){

  //  int max_steps = 15;  // default
  int max_steps = MAX_STEPS;  // default

  max_steps = 5;  // rough estimation: 1 kick, 2 turns, then dash

  bool with_kick = false;
  bool target_is_ball = true;
  bool stop_if_turned = true;
  kickspeed = 0.0;

  if((targetdir.diff(myang) <=BODYDIR_TOLERANCE/180.*PI)){
    DBLOG_MOV(0,"Hey, I already turned in that direction! targetdir "<<RAD2DEG(targetdir.get_value()));
    max_steps = 2; // try a dash
    stop_if_turned = 0; // don't stop if direction ok -> this is already true
  }

  
  //first, check, without kick movement
  static const float minDistToBall = 1.25*(ServerOptions::player_size + ServerOptions::ball_size);
  simulate_my_movement(targetdir,max_steps,simulation_table,mypos, myvel, myang, mystamina, ballpos, ballvel,
		       with_kick, stop_if_turned); 
  bool tooCloseToBall = simulation_table[1].my_pos.distance(simulation_table[1].ball_pos)<minDistToBall;
  if(!tooCloseToBall){
    simulate_ops_movement(simulation_table, target_is_ball);
    print_table(simulation_table);
    if(check_nokick_selfpass(simulation_table, targetpos, actual_steps, attacking_op, op_number,ballpos) == true){
      if((targetdir.diff(simulation_table[actual_steps].my_bodydir) <=BODYDIR_TOLERANCE/180.*PI)){
	DBLOG_MOV(0,"Hey, I can turn2dir safely without kick. targetdir "<<RAD2DEG(targetdir.get_value()));
	return true;
      }
    }
  } // not too Close to Ball

  // now check with kick
  with_kick = true;
  target_is_ball = false;
  simulate_my_movement(targetdir,max_steps,simulation_table, mypos, myvel, myang, mystamina, ballpos, ballvel,
		       with_kick, stop_if_turned); 
  simulate_ops_movement(simulation_table, target_is_ball);

  print_table(simulation_table);


  if(  determine_kick(simulation_table, targetdir, targetpos, kickspeed, actual_steps, attacking_op, op_number,
		      mypos, myvel, myang, mystamina, ballpos, ballvel) == true){
    if(targetdir.diff(simulation_table[actual_steps].my_bodydir) <=BODYDIR_TOLERANCE/180.*PI){ 
      // at final position, I do look into target direction 
      DBLOG_MOV(0,"Turn 2 Dir Is Safe: Hey, I found a correct kick command to dir "<<RAD2DEG(targetdir.get_value())
		<<" mypos "<<mypos<<" targetpos "<<targetpos<<" kickspeed "<<kickspeed);
      return true;
    }
    else{ // turn not possible
      DBLOG_MOV(0,"Turn 2 dir: I found a correct kick command, but turning not possible in time");
      return false;
    }
  }
  else{ // no kick found
    DBLOG_MOV(0,"Turn2dir Is Safe: Couldn't find an appropriate one step kick to dir "<<RAD2DEG(targetdir.get_value()));
    return false;
  }
  return false;
}


bool Selfpass2::is_selfpass_safe(const ANGLE targetdir, Value &kickspeed, Vector &targetpos, int &actual_steps,
				 Vector &attacking_op, int & op_number, 
				 const bool check_nokick_only, const int reduce_dashes){
  return is_selfpass_safe(targetdir, kickspeed, targetpos, actual_steps, attacking_op, op_number,
			  WSinfo::me->pos, WSinfo::me->vel, WSinfo::me->ang, WSinfo::me->stamina,
			  WSinfo::ball->pos, WSinfo::ball->vel, check_nokick_only, reduce_dashes);

}



bool Selfpass2::is_selfpass_safe(const ANGLE targetdir, Value &kickspeed, Vector &targetpos, int &actual_steps,
				 Vector &attacking_op, int & op_number, 
				 const Vector mypos, const Vector myvel, const ANGLE myang,
				 const Value mystamina,
				 const Vector ballpos, const Vector ballvel,
				 const bool check_nokick_only, const int reduce_dashes){

  //  int max_steps = 15;  // default
  int max_steps = MAX_STEPS;  // default

  bool with_kick = false;
  bool target_is_ball = true;
  kickspeed = 0.0;

  static const float minDistToBall = 1.25*(ServerOptions::player_size + ServerOptions::ball_size);

  //first, check, without kick movement
  simulate_my_movement(targetdir,max_steps,simulation_table,mypos, myvel, myang, mystamina, 
		       ballpos, ballvel, with_kick); 
	
  bool tooCloseToBall = simulation_table[1].my_pos.distance(simulation_table[1].ball_pos)<minDistToBall;

  if(!tooCloseToBall){
    simulate_ops_movement(simulation_table, target_is_ball);
    print_table(simulation_table);
    
    if(check_nokick_selfpass(simulation_table, targetpos, actual_steps,  attacking_op, op_number, ballpos) == true){
      if((targetdir.diff(simulation_table[actual_steps].my_bodydir) 
	  <=BODYDIR_TOLERANCE/180.*PI)){
	// I am looking to the right direction
	if (mypos.distance(targetpos) > 3.0){
	  // I can advance very nicely
	  DBLOG_MOV(0,"Hey, I can considerably move without kick in dir "<<RAD2DEG(targetdir.get_value()));
	  return true;
	}
	if(check_nokick_only == true){
	  DBLOG_MOV(0,"Hey, I only test withoutkick, and at least can turn 2 dir "<<RAD2DEG(targetdir.get_value()));
	  return true;
	}
      } // after all, I am looking in targetdir
    } // check nokick selfpass is true
  } // not too close too ball


  if(check_nokick_only == true){
    DBLOG_MOV(0,"Hey, Check with nokick only not successful, return false");
    // hmm, withoutkick seems not to be a success!!!
    return false;
  }

  DBLOG_MOV(0,"Is Safe: not possible without kick in dir "<<RAD2DEG(targetdir.get_value()));

  // now check with kick
  with_kick = true;
  target_is_ball = false;
  simulate_my_movement(targetdir,max_steps,simulation_table,mypos, myvel, myang, mystamina, ballpos, ballvel, with_kick); 
  simulate_ops_movement(simulation_table, target_is_ball);

  print_table(simulation_table);


  if(  determine_kick(simulation_table, targetdir, targetpos, kickspeed, actual_steps, attacking_op, op_number, 
		      mypos, myvel, myang, mystamina, ballpos, ballvel,reduce_dashes) == true){
    if(targetdir.diff(simulation_table[actual_steps].my_bodydir) <=BODYDIR_TOLERANCE/180.*PI){ 
      // at final position, I do look into target direction 
      DBLOG_MOV(0,"Is Safe: Hey, I found a correct kick command to dir "<<RAD2DEG(targetdir.get_value())
		<<" targetpos "<<targetpos<<" kickspeed "<<kickspeed);
      return true;
    }
    else{ // turn not possible
      DBLOG_MOV(0,"I found a correct kick command, but turning not possible in time");
      return false;
    }
  }
  else{ // no kick found
    DBLOG_MOV(0,"Is Safe: Couldn't find an appropriate one step kick to dir "<<RAD2DEG(targetdir.get_value()));
    return false;
  }
  return false;
}

void Selfpass2::get_cmd_to_go2pos(Cmd &tmp_cmd,const Vector targetpos,const Vector pos, const Vector vel, 
				  const ANGLE bodydir, const int stamina,
				  const PPlayer player){

  if(pos.distance(targetpos) < 2*ServerOptions::player_speed_max){
    // potentially could reach the targetpos by simply dashing; so try this first!
    Vector next_pos, next_vel;
    ANGLE next_bodydir;
    int next_stamina;

    tmp_cmd.cmd_main.unset_lock();
    tmp_cmd.cmd_main.unset_cmd();
    basic_cmd->set_dash(100);
    basic_cmd->get_cmd(tmp_cmd);	
    Tools::simulate_player(pos,vel,bodydir,stamina,
			   tmp_cmd.cmd_main, 
			   next_pos,next_vel,next_bodydir,next_stamina,
			   player->stamina_inc_max, player->inertia_moment,
			   player->dash_power_rate,
			   player->effort, player->decay);
    if(at_position(next_pos, next_bodydir, player->kick_radius, targetpos)){
      // hey, a dash will do this!!
      return;
    }
    tmp_cmd.cmd_main.unset_lock();
    tmp_cmd.cmd_main.unset_cmd();
  }

  // normal computation
  ANGLE targetdir;
  targetdir = (targetpos - pos).ARG();
  get_cmd_to_go2dir(tmp_cmd, targetdir, pos,vel,bodydir, stamina, player->inertia_moment, 
		    player->stamina_inc_max);
}


void Selfpass2::get_cmd_to_go2dir(Cmd &tmp_cmd,const ANGLE targetdir,const Vector pos, const Vector vel, 
				  const ANGLE bodydir, const int stamina,
				  const Value inertia_moment, const Value stamina_inc_max){
  // compute right command to dash in targetdir
  Value moment;

  if(targetdir.diff(bodydir) >BODYDIR_TOLERANCE/180.*PI){ // have to turn
    Value moment = (targetdir-bodydir).get_value_mPI_pPI() * 
      (1.0 + (inertia_moment * (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);
    return;
  }
  
  // turned, now dashing
  int dash_power = 100;
  if(stamina <= ServerOptions::recover_dec_thr*ServerOptions::stamina_max + 100.){
    dash_power = (int)stamina_inc_max;
  }
  basic_cmd->set_dash(dash_power);
  basic_cmd->get_cmd(tmp_cmd);	
  return;
}

void Selfpass2::reset_simulation_table(Simtable *simulation_table){
  for(int i=0; i<MAX_STEPS; i++){
    simulation_table[i].valid_at = -1; // invalidate
  }
}


void Selfpass2::simulate_my_movement(const ANGLE targetdir, const int max_steps, Simtable *simulation_table, 
				 const Vector mypos, const Vector myvel, const ANGLE myang,
				 const Value mystamina,
				 const Vector ballpos, const Vector ballvel,
				     const bool with_kick, const bool turn2dir_only){
  // creates a table with possible positions, when first cmd is a kick
  Cmd tmp_cmd;

  Vector tmp_pos = mypos;
  Vector tmp_vel = myvel;
  Vector tmp_ballvel = ballvel;  
  Vector tmp_ballpos = ballpos;  
  Vector next_pos, next_vel, next_ballpos, next_ballvel;
  ANGLE tmp_bodydir = myang;
  Angle next_bodydir;
  int tmp_stamina = (int) mystamina;
  int next_stamina;

  reset_simulation_table(simulation_table);

  for(int i=0; i<=max_steps && i<MAX_STEPS; i++){
    tmp_cmd.cmd_main.unset_lock();
    tmp_cmd.cmd_main.unset_cmd();
    if(i==0 && with_kick == true){
      // do nothing here; simulate a kick command, which is currently not known.
      basic_cmd->set_turn(0);
      basic_cmd->get_cmd(tmp_cmd); // copy it to tmp_cmd;
    }
    else 
      get_cmd_to_go2dir(tmp_cmd, targetdir,tmp_pos,tmp_vel,tmp_bodydir, tmp_stamina, WSinfo::me->inertia_moment, WSinfo::me->stamina_inc_max);

    // write to table:
    simulation_table[i].valid_at = WSinfo::ws->time; // currently valid
    simulation_table[i].my_pos = tmp_pos;
    simulation_table[i].my_vel = tmp_vel;
    simulation_table[i].my_bodydir = tmp_bodydir;
    simulation_table[i].ball_pos = tmp_ballpos;
    simulation_table[i].ball_vel = tmp_ballvel;
    simulation_table[i].cmd = tmp_cmd;
    if(tmp_pos.distance(tmp_ballpos) <= WSinfo::me->kick_radius - SAFETY_MARGIN)