dribble_straight_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 "dribble_straight_bms.h"
#include "tools.h"
#include "options.h"
#include "ws_info.h"
#include "log_macros.h"

#define BASELEVEL 1

//#define DBLOG_MOV(LLL,XXX) LOG_MOV(LLL,<<"DribbleStraight: "<<XXX)
#define DBLOG_MOV_URGENT(LLL,XXX) LOG_MOV(LLL,<<"DribbleStraight: "<<XXX)
#define DBLOG_MOV(LLL,XXX) 
//#define DBLOG_DRAW(LLL,XXX) LOG_POL(LLL,<<_2D<<XXX)
#define DBLOG_DRAW(LLL,XXX)

bool DribbleStraight::initialized=false;

#define PROT(XXX)(std::cout<<XXX)
//#define PROT(XXX)
#define SAFETY_MARGIN 0.2

DribbleStraight::DribbleStraight(){
  //const Value safety_margin = 0.15; //0.1 is too small

  //success_sqrdist = SQUARE(WSinfo::me->kick_radius - SAFETY_MARGIN);

  onetwokick = new OneOrTwoStepKick;
  onestepkick = new OneStepKick;
}

//Value DribbleStraight::success_sqrdist;

bool DribbleStraight::init(char const * conf_file, int argc, char const* const* argv){
  if(initialized) return true;

  bool res = OneOrTwoStepKick::init(conf_file,argc,argv) &&
    OneStepKick::init(conf_file,argc,argv);
  if(!res) exit(1);

  //success_sqrdist = SQUARE(WSinfo::me->kick_radius - SAFETY_MARGIN);
  
  initialized = true;
  INFO_OUT << "\nDribbleStraight behavior initialized.";
  return initialized;
}

bool DribbleStraight::is_dribble_safe_old() {
  //#define HOLDTURN_SAFE_DIST .8

  Cmd cmd;
  //Value dist;
  //int steps;

  MyState state;
  
  state.my_pos = WSinfo::me->pos;
  state.my_vel = WSinfo::me->vel;
  state.ball_pos = WSinfo::ball->pos;
  state.ball_vel = WSinfo::ball->vel;
  state.my_angle = WSinfo::me->ang;

  WSpset pset= WSinfo::valid_opponents;
  pset.keep_and_sort_closest_players_to_point(1, state.ball_pos);
  if ( pset.num ){
    state.op_pos= pset[0]->pos;
    state.op_bodydir =pset[0]->ang;
    state.op_bodydir_age = pset[0]->age_ang;
    DBLOG_MOV(0,"HoldTurn: Age of Angle: "<<pset[0]->age_ang);
  }
  else{
    state.op_pos= Vector(1000,1000); // outside pitch
    state.op_bodydir = ANGLE(0);
    state.op_bodydir_age = 1000;
  }


  return false;
}

bool DribbleStraight::ballpos_ok(const MyState &state){
  if(!Tools::is_position_in_pitch(state.ball_pos)) return false;
  Value sqrdist = state.my_pos.sqr_distance(state.ball_pos);
  if(sqrdist > SQUARE(WSinfo::me->kick_radius - SAFETY_MARGIN)) {
    //DBLOG_MOV(0,"ballpos not ok: out of kickrange");
    return false;
  }
  if(sqrdist < SQUARE(WSinfo::me->radius+.2)) {
    //DBLOG_MOV(0,"ballpos not ok: collision");
    return false; // collision ?
  }
  return true;
  //return Tools::is_ball_safe_and_kickable(state.my_pos, state.op_pos, state.op_bodydir, state.ball_pos, state.op_bodydir_age);
}

bool DribbleStraight::ballpos_optimal(const MyState &state){
  //return false; // currently deactivated takes too long
  Value sqrdist = state.my_pos.sqr_distance(state.ball_pos);
  if(sqrdist > SQUARE(WSinfo::me->kick_radius - SAFETY_MARGIN))
    return false;
  if (state.ball_pos.sqr_distance(state.my_pos)< SQUARE(0.4)) // keep it before you
    return false;

  Angle delta_dir = (state.ball_pos - state.my_pos).arg();
  delta_dir = Tools::get_angle_between_mPI_pPI(delta_dir);
  if(delta_dir <-90./180.*PI)
    return false;
  if(delta_dir >90./180.*PI)
    return false;
  return true;
}

bool DribbleStraight::get_cmd(Cmd & cmd){
  
  //return get_cmd(cmd, opponent_goalpos);

  MyState state;
  state.get_from_WS();
#if 0
  WSpset pset= WSinfo::valid_opponents;
  pset.keep_and_sort_closest_players_to_point(1, state.ball_pos);
  if ( pset.num ){
    state.op_pos= pset[0]->pos;
    state.op_bodydir =pset[0]->ang;
    state.op_bodydir_age = pset[0]->age_ang;
    DBLOG_MOV(0,"DribbleStraight: Age of Angle: "<<pset[0]->age_ang);
  }
  else{
    state.op_pos= Vector(1000,1000); // outside pitch
    state.op_bodydir = ANGLE(0);
    state.op_bodydir_age = 1000;
  }
#endif  
  return calc_next_cmd(cmd,state);
}

bool DribbleStraight::calc_next_cmd(Cmd &next_cmd, const MyState &cur_state) {
#define SAFETY 0.15
#define MAX_STATES 20
#define STAMINA_THRESHOLD 1400

  if(!WSinfo::is_ball_kickable()) return false;
  if(WSinfo::ws->play_mode==PM_PlayOn && last_calc==WSinfo::ws->time) {
    next_cmd.cmd_main.clone(cached_cmd);
    return cached_res;
  }
  last_calc=WSinfo::ws->time;
  cached_cmd.unset_lock();cached_cmd.unset_cmd();
  cached_res=false;
  
  Cmd dashcmd,dumcmd;
  MyState state[MAX_STATES];MyState tmpstate;
  Vector vec2target;

  Value maxdash=100;
  if(WSinfo::me->stamina<STAMINA_THRESHOLD) {
    DBLOG_MOV(0,"Activating stamina saving mode...");
    maxdash=WSinfo::me->stamina_inc_max;
  }
  
  dashcmd.cmd_main.set_dash(maxdash);
  dumcmd.cmd_main.set_turn(0);

  ANGLE target_dir=WSinfo::me->ang;

  bool place_ball = false;

  Tools::get_successor_state(cur_state,dumcmd.cmd_main,state[0]);
  DBLOG_DRAW(0,C2D(state[0].my_pos.x,state[0].my_pos.y,0.3,"red"));
  DBLOG_DRAW(0,C2D(state[0].my_pos.x,state[0].my_pos.y,WSinfo::me->kick_radius,"red"));
  for(int i=1;i<MAX_STATES;i++) {
    Tools::get_successor_state(state[i-1],dashcmd.cmd_main,state[i]);
    DBLOG_DRAW(0,C2D(state[i].my_pos.x,state[i].my_pos.y,0.3,"blue"));
    DBLOG_DRAW(0,C2D(state[i].my_pos.x,state[i].my_pos.y,WSinfo::me->kick_radius,"blue"));
  }
  
  Vector vec2ball = (state[0].ball_pos - state[0].my_pos);
  vec2ball.rotate(-state[0].my_angle.get_value());
  Value ydist = vec2ball.y;
  bool dribble_left = (ydist>=0 ? true : false);
  ydist = fabs(ydist);
  DBLOG_MOV(0,"ydist: "<<ydist<<", kick_radius: "<<WSinfo::me->kick_radius);

  //Value max_op_radius = ServerOptions::tackle_dist+ServerOptions::player_speed_max+SAFETY;
  Value max_op_radius = 4; // just to be sure...
  bool must_kick=false;
  
  // what happens if we do not kick the ball (and dash instead)?

  WSpset opnearball=WSinfo::valid_opponents;
  opnearball.keep_players_in_circle(state[0].ball_pos,max_op_radius);
  if(opnearball.num>0) { // check if ball is still safe if we do not kick...
    DBLOG_MOV(0,"Opponents may be in range... checking!");
    for(int p=0;p<opnearball.num;p++) {
      DBLOG_MOV(0,<< _2D << C2D(opnearball[p]->pos.x, opnearball[p]->pos.y ,1.4,"red")); 
      //      if(!Tools::is_ballpos_safe(opnearball[p],state[0].ball_pos)) {
      if(!Tools::is_ballpos_safe(opnearball[p],state[0].ball_pos,true)) { // check w. tackle possibility
	DBLOG_MOV(0,"Opp #"<<opnearball[p]->number<<" may get the ball if we don't kick!");
	must_kick=true;
	break;
      }
    }
  }

  if(!must_kick) {  // ok, so we try to dash first!
    Tools::get_successor_state(cur_state,dashcmd.cmd_main,tmpstate);
    if(ballpos_ok(tmpstate)) {
      DBLOG_MOV(0,"Ballpos ok, dashing...");
      next_cmd.cmd_main.clone(dashcmd.cmd_main);
      cached_cmd.clone(dashcmd.cmd_main);
      cached_res=true;
      return true;
    }
  }
  // hmm, so we must kick...
  DBLOG_MOV(0,"We need to kick now! Searching best kick move...");
  
  Cmd_Main bestcmd;
  int maxsteps=-1;
  Vector cur_pos,cur_vel;
  Vector bestpos,bestvel;
  Value bestdist=0;
  DribbleStraightItrActions itr_actions;

  itr_actions.reset();
  while(Cmd_Main *action = itr_actions.next()) {
    bool all_ok = true;
    Tools::get_successor_state(cur_state,*action,state[0]);
    ///* always kick more or less parallel to body ang... */
    // INSERT APPROPRIATE CODE HERE //
    if(!ballpos_ok(state[0])) continue;
    for(int p=0;p<opnearball.num;p++) {
      DBLOG_MOV(0,<< _2D << C2D(opnearball[p]->pos.x, opnearball[p]->pos.y ,1.4,"red")); 
      //      if(!Tools::is_ballpos_safe(opnearball[p],state[0].ball_pos)) {
      if(!Tools::is_ballpos_safe(opnearball[p],state[0].ball_pos,true)) { // check w. tackle possibily
	all_ok=false;break;