dribble_straight_bms.c

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

      }
    }
    if(!all_ok) continue;
    // we have found a kick move that keeps the ball in kickrange AND leads to a safe position!
    int steps;
    cur_pos=state[0].ball_pos;
    cur_vel=state[0].ball_vel;
    for(steps=1;steps<MAX_STATES;steps++) {
      cur_pos+=cur_vel;
      state[steps].ball_pos = cur_pos;
      cur_vel*=ServerOptions::ball_decay;
      state[steps].ball_vel = cur_vel;
      if(!ballpos_ok(state[steps])) {
	//steps--;
	break;
      }
    }
#if 0   // step based
    if(steps>=maxsteps) {
      Value dist=(state[steps-1].ball_pos-cur_state.ball_pos).norm();
      if(steps>maxsteps || dist>bestdist) {
	maxsteps=steps;
	bestcmd.unset_lock();bestcmd.unset_cmd();
	bestcmd.clone(*action);
	bestpos=state[0].ball_pos;
	bestvel=state[0].ball_vel;
	bestdist=dist;
      }
    }
#else
    // dist based
    Vector ballvec=state[steps-1].ball_pos-cur_state.ball_pos;
    ballvec.rotate(-WSinfo::me->ang.get_value());
    Value dist=ballvec.x;
    if(dist>=bestdist) {
      bestdist=dist;
      maxsteps=steps;
      bestcmd.unset_lock();bestcmd.unset_cmd();
      bestcmd.clone(*action);
      bestpos=state[0].ball_pos;
      bestvel=state[0].ball_vel;
    }
#endif
  }
  if(bestdist>0) {
    DBLOG_MOV(0,"Found a suitable kick move ("<<maxsteps<<" steps, dist="<<bestdist<<")");
    Vector cur_vel,cur_pos;
    cur_vel=bestvel;
    cur_pos=bestpos;
    for(int i=0;i<maxsteps;i++) {
      DBLOG_DRAW(0,C2D(cur_pos.x,cur_pos.y,ServerOptions::ball_size,"black"));
      cur_pos+=cur_vel;
      cur_vel*=ServerOptions::ball_decay;
    }
    next_cmd.cmd_main.clone(bestcmd);
    cached_cmd.clone(bestcmd);
    cached_res=true;
    return true;
  } else {
    DBLOG_MOV_URGENT(0,"Could not find any suitable kick action! DribbleStraight not possible!");
    //std::cerr << "\nCould not find a dribble action!";
    return false;
  }
}
  
bool DribbleStraight::is_dribble_safe(int opp_time2react) {
  //MyState state;
  //state.get_from_WS();
  Cmd dumcmd;
  
  return get_cmd(dumcmd);
}



bool DribbleStraight::is_dribble_safe( const AState & state, int opp_time2react ) {

  Cmd dumcmd;
  
  MyState my_state;

  my_state.my_vel = state.my_team[state.my_idx].vel;
  my_state.my_pos = state.my_team[state.my_idx].pos;
  my_state.ball_vel = state.ball.vel;
  my_state.ball_pos = state.ball.pos;
  my_state.my_angle = ANGLE(state.my_team[state.my_idx].body_angle);

  return calc_next_cmd(dumcmd,my_state);
}

























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

  if(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();
  
  Cmd tmpcmd,tmpcmd2;
  MyState state[MAX_STATES];
  MyState tmpstate;
  Vector vec2target;

  tmpcmd.cmd_main.set_dash(100);
  tmpcmd2.cmd_main.set_turn(0);

  ANGLE target_dir=WSinfo::me->ang;

  bool place_ball = false;

  Tools::get_successor_state(cur_state,tmpcmd2.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],tmpcmd.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);

  // do we need to place the ball into its corridor?
 
  if(ydist<WSinfo::me->radius+SAFETY || ydist > .9 || !ballpos_ok(cur_state)) {
    // already in corridor?
    Vector dum=(WSinfo::ball->pos - WSinfo::me->pos);
    dum.rotate(-WSinfo::me->ang.get_value());
    dum.y=fabs(dum.y);
    
    vec2target.y = WSinfo::me->radius+ServerOptions::ball_size+SAFETY; 
    vec2target.x = -.5*sqrt(SQUARE(WSinfo::me->kick_radius-SAFETY)-SQUARE(vec2target.y));
    //vec2target.x=0;
    //vec2target.y = WSinfo::me->radius+ServerOptions::ball_size+SAFETY;
    if(!dribble_left) vec2target.y*=-1;
    vec2target.ROTATE(state[0].my_angle);
    if(dum.y<WSinfo::me->radius+SAFETY || dum.y > .9 /*|| !ballpos_ok(cur_state)*/) {
      DBLOG_MOV(0,"Putting ball into corridor...");
      place_ball = true;
      vec2target+=state[0].my_pos;
      DBLOG_DRAW(0,C2D(vec2target.x,vec2target.y,.5,"gray"));
      target_dir=(vec2target-WSinfo::ball->pos).ARG();
    } else {
      //DBLOG_MOV(0,"Correcting target_dir...");
      //vec2target+=state[5].my_pos;
      //target_dir=(vec2target-WSinfo::ball->pos).ARG();
    }
  } else {
  
    // dash possible?
    
    Tools::get_successor_state(cur_state,tmpcmd.cmd_main,tmpstate);
    if(/*!place_ball &&*/ ballpos_ok(tmpstate)) {
      DBLOG_MOV(0,"Ballpos ok, dashing...");
      next_cmd.cmd_main.clone(tmpcmd.cmd_main);
      cached_cmd.clone(tmpcmd.cmd_main);
      cached_res=true;
      return true;
    }
  }

  // kick!
 
  DBLOG_MOV(0,"Kicking...");

  // find target dir

  Value bestvel=0;
  int maxsteps=-1;
  Value maxvel = onestepkick->get_max_vel_in_dir(target_dir,false);
  if(place_ball) {
    bestvel = (vec2target-WSinfo::ball->pos).norm();
    maxsteps = 1;
  } else {
    for(Value vel=0.1;vel<=maxvel;vel+=.1) {
      Vector cur_vel,cur_pos;
      cur_vel.init_polar(vel,target_dir);
      cur_pos=WSinfo::ball->pos;
      int steps;
      for(steps=0;steps<MAX_STATES;steps++) {
	cur_pos+=cur_vel;
	state[steps].ball_pos = cur_pos;
	cur_vel*=ServerOptions::ball_decay;
	state[steps].ball_vel = cur_vel;
	if(!ballpos_ok(state[steps])) {
	  steps--;
	  break;
	}
      }
      if(steps>=maxsteps) {
	maxsteps=steps; bestvel=vel;
      }
    }
  }
  DBLOG_MOV(0,"maxsteps: "<<maxsteps<<", bestvel: "<<bestvel<< ", maxvel: "<<maxvel);
  if(bestvel>0 && maxsteps>=0) {
    Vector cur_vel,cur_pos;
    cur_vel.init_polar(bestvel,target_dir);
    cur_pos=WSinfo::ball->pos;
    for(int i=0;i<=maxsteps;i++) {
      DBLOG_DRAW(0,C2D(cur_pos.x,cur_pos.y,ServerOptions::ball_size,"black"));
      cur_pos+=cur_vel;
      cur_vel*=ServerOptions::ball_decay;
    }
    if(place_ball) {
      onestepkick->kick_to_pos_with_initial_vel(bestvel,vec2target);
    } else {
      onestepkick->kick_in_dir_with_initial_vel(bestvel,target_dir);
    }
    onestepkick->get_cmd(next_cmd);
    cached_cmd.clone(next_cmd.cmd_main);
    cached_res=true;
    return true;
  } else {
    DBLOG_MOV(0,"Could not find a dribble action!");
    std::cerr << "\nCould not find a dribble action!";
  }
  cached_cmd.set_turn(0);
  cached_res=false;
  return false;
}
#endif