selfpass2_bms.c

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

      simulation_table[i].I_have_ball = true;
    else
      simulation_table[i].I_have_ball = false;
    if(turn2dir_only == true){
      if((targetdir.diff(simulation_table[i].my_bodydir) <=BODYDIR_TOLERANCE/180.*PI)){
	// only do steps, until direction is ok.
	return;
      }
    }

    Tools::model_cmd_main(tmp_pos,tmp_vel,tmp_bodydir.get_value(),tmp_stamina, tmp_ballpos, tmp_ballvel,
			  tmp_cmd.cmd_main, 
			  next_pos,next_vel,next_bodydir,next_stamina, next_ballpos, next_ballvel);
      
    tmp_pos = next_pos;
    tmp_vel = next_vel;
    tmp_ballpos = next_ballpos;
    tmp_ballvel = next_ballvel;
    tmp_bodydir = ANGLE(next_bodydir);
    tmp_stamina = next_stamina;
  };  // while
}

bool Selfpass2::at_position(const Vector playerpos, const ANGLE bodydir, const Value kick_radius, const Vector targetpos){
   if(playerpos.distance(targetpos) <=  kick_radius) // at position!
     return true;
   Vector translation, new_center;
   translation.init_polar(0.5, bodydir);
   new_center = playerpos + translation;
   if(new_center.distance(targetpos) <= kick_radius) // at position!
     return true;
   return false;
}


int Selfpass2::get_min_cycles2_pos(const Vector targetpos, const PPlayer player, const int max_steps, Vector &resulting_pos){

  Cmd tmp_cmd;
  Vector tmp_pos = player->pos;
  Vector tmp_vel = player->vel;
  Vector next_pos, next_vel;
  ANGLE tmp_bodydir = player->ang;
  ANGLE next_bodydir;
  int tmp_stamina = (int) player->stamina;
  int next_stamina;

  // ridi: make it more safe:
  if (player->age_ang >0){
    tmp_bodydir = (targetpos - tmp_pos).ARG();
  }
  // probably too cautious...
  /*
  if (player->age_vel >0){
    Vector vel;
    vel.init_polar(ServerOptions::player_speed_max, (targetpos - tmp_pos).ARG());
    tmp_vel = vel;
  }
  */
  resulting_pos = tmp_pos;

  int i;
  if(player->number == 0){
    DBLOG_MOV(0,"checking op player "<<player->number<<" kick radius "<<player->kick_radius
	      <<" inertia_moment "<<player->inertia_moment<<" power rate "<<player->dash_power_rate
	      <<" effort "<<player->effort<<" decay "<<player->decay);
  }

  if(tmp_pos.distance(targetpos) > (max_steps+2) * 1.25 * ServerOptions::player_speed_max + player->kick_radius){
    // no chance to get to position in time
    //    DBLOG_DRAW(0,C2D(tmp_pos.x,tmp_pos.y,1.3,"black"));          
    return -1;
  }

  for(i=0; i<=max_steps && i<=MAX_STEPS; i++){
    // test only: remove or deactivate
    /*
    char infoString[10];
    sprintf(infoString,"%d",max_steps); // this denotes the trajecetory number
    DBLOG_DRAW(0,STRING2D(tmp_pos.x-0.2,tmp_pos.y-0.4,infoString,"ffff00"));
    DBLOG_DRAW(0,C2D(next_pos.x,next_pos.y,player->kick_radius,"ffff00"));      
    */
    // test only: end

    resulting_pos = tmp_pos;
    Value radius = player->kick_radius;
    if(player == WSinfo::his_goalie)
      radius = ServerOptions::catchable_area_l;

    if(at_position(tmp_pos, tmp_bodydir, radius ,targetpos) == true){
      // hey, I am already at position
      return i;
    }
    tmp_cmd.cmd_main.unset_lock();
    tmp_cmd.cmd_main.unset_cmd();

    get_cmd_to_go2pos(tmp_cmd, targetpos,tmp_pos,tmp_vel,tmp_bodydir, tmp_stamina, player);

    Tools::simulate_player(tmp_pos,tmp_vel,tmp_bodydir,tmp_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);

    tmp_pos = next_pos;
    tmp_vel = next_vel;
    tmp_bodydir = ANGLE(next_bodydir);
    tmp_stamina = next_stamina;
  };  // while
  return -1;
}

void Selfpass2::simulate_ops_movement(Simtable *simulation_table, const bool target_is_ball){
  // for all steps, for all ops: check time to playerpos
  
  // check opponents;
  WSpset pset = WSinfo::valid_opponents; // enough, if I do it once.
  Vector closest_op;
  Value closest_dist = 1000;

  for(int t=0; t<MAX_STEPS; t++){
    if(simulation_table[t].valid_at != WSinfo::ws->time){
      // entry not valid
      break;
    }
    Vector targetpos = simulation_table[t].my_pos;
    if(target_is_ball == true){
      targetpos = simulation_table[t].ball_pos;
    }
    int best_op_steps = 1000;
    Vector best_op_resulting_pos;
    int closest_op_num;
    int best_idx = -1;

    for (int idx = 0; idx < pset.num; idx++) {
      int max_steps = t; // check for t steps only
      Vector resulting_pos;
      int op_steps = get_min_cycles2_pos(targetpos, pset[idx], max_steps, resulting_pos);

      if(op_steps >=0 && op_steps < best_op_steps){ // I found an opponent that gets to position in time
	best_op_steps = op_steps;
	best_op_resulting_pos = resulting_pos;
	best_idx = idx;
	closest_dist = 0;
      }
      if(targetpos.distance(resulting_pos) < closest_dist){
	closest_dist = targetpos.distance(resulting_pos);
	closest_op = pset[idx]->pos;
	closest_op_num = pset[idx]->number;
      }

    } // for all ops idx


    if(best_idx >=0){ // found an intercepting opponent
      simulation_table[t].op_pos = pset[best_idx]->pos;;
      simulation_table[t].op_steps2pos = best_op_steps;
      simulation_table[t].op_num = pset[best_idx]->number;
    }
    else{
      simulation_table[t].op_pos = closest_op;
      simulation_table[t].op_num = closest_op_num;
      simulation_table[t].op_steps2pos = -1;
    }
  }// for all t
}


void Selfpass2::print_table(Simtable *simulation_table){
  for(int t=0; t<MAX_STEPS; t++){
    if(simulation_table[t].valid_at != WSinfo::ws->time){
      // entry not valid
      break;
    }
    DBLOG_MOV(1,"time "<<t<<" mypos: "<<simulation_table[t].my_pos
	      <<" closest op: "<<simulation_table[t].op_pos
	      <<" number "<<simulation_table[t].op_num
	      <<" gets me in "<<simulation_table[t].op_steps2pos<<" steps"<<" have ball: "<<simulation_table[t].I_have_ball);

    // oppos
    char infoString[10];
    sprintf(infoString,"%d",simulation_table[t].op_steps2pos);
    /*
    if(simulation_table[t].op_steps2pos >0){
      DBLOG_DRAW(0,STRING2D(simulation_table[t].op_pos.x-0.2,simulation_table[t].op_pos.y-0.4,infoString,"ff0000")
		 <<C2D(simulation_table[t].op_pos.x,simulation_table[t].op_pos.y,1.1,"ff0000"));
    }
    */

    // mypos
    /*
    sprintf(infoString,"%d",t);

    DBLOG_DRAW(0,C2D(simulation_table[t].my_pos.x,simulation_table[t].my_pos.y,0.1,"0000ff")
	       <<STRING2D(simulation_table[t].my_pos.x-0.2,simulation_table[t].my_pos.y-0.4,infoString,"0000ff"));
    */

  }

  Value turn_angle = 0;
  Value dash_power = 0;
  Value kick_power = 0;
  Value kick_angle = 0;

  switch(simulation_table[0].cmd.cmd_main.get_type()){
  case Cmd_Main::TYPE_DASH:
    simulation_table[0].cmd.cmd_main.get_dash(dash_power);
    DBLOG_MOV(1,"DASH "<<dash_power);
    break;
  case Cmd_Main::TYPE_KICK:
    simulation_table[0].cmd.cmd_main.get_kick(kick_power, kick_angle);
    DBLOG_MOV(1,"KICK power "<<kick_power<<" kick dir "<<RAD2DEG(kick_angle));
  break;
  case Cmd_Main::TYPE_TURN:
    simulation_table[0].cmd.cmd_main.get_turn(turn_angle);
    DBLOG_MOV(1,"TURN "<<turn_angle);
    break;
  }
}


bool Selfpass2::determine_kick(Simtable *simulation_table, const ANGLE targetdir,
			       Vector & targetpos, Value & targetspeed, int & steps, Vector & attacking_op, 
			       int & attacking_num, 
			       const Vector mypos, const Vector myvel, const ANGLE myang,
			       const Value mystamina,
			       const Vector ballpos, const Vector ballvel,
			       const int reduce_dashes ){
  int best_t = -1;

  Value current_dist2border = Tools::min_distance_to_border(mypos);

  for(int t=1; t<MAX_STEPS; t++){  // start with t=1, since I know already that I have the ball at t= 0 (NOW!)
    if(simulation_table[t].valid_at != WSinfo::ws->time){
      // entry not valid
      break;
    }
    if(simulation_table[t].op_steps2pos >= 0 && simulation_table[t].op_steps2pos <= t){ 
      // if opponent gets me at all and he is faster or equally fast at position, stop search.
      // critical point: break or no break:
      // break;  // using break here is the more safe version; probably uses spectacular dribblings
    }
    else{ // opponent does not get to position in time, now check if its inside pitch
      if((Tools::min_distance_to_border(simulation_table[t].my_pos) > MIN_DIST_2_BORDER) ||
	 (current_dist2border < MIN_DIST_2_BORDER && 
	  Tools::min_distance_to_border(simulation_table[t].my_pos) > current_dist2border)){
	// position is either in pitch, or improves my current situation
	best_t = t;
      }
    }
  }
  

  best_t -= reduce_dashes;  // for a safer life!


  if (best_t <0){ // no position is safe
    steps = -1;
    return false;
  } 

  attacking_op = simulation_table[best_t].op_pos;
  attacking_num = simulation_table[best_t].op_num;
  //  DBLOG_MOV(0,"check with kick selfpass: Attackerpos: "<<attacking_op);

  // found a safe position
  Value summed_decay = 0.0;
  Value decay = 1.0;
  for(int t = 0; t<=best_t;t++){ // compute decay; not elegant, but explicit and clear
    summed_decay += decay;
    decay *= ServerOptions::ball_decay;
  };  

  targetpos = simulation_table[best_t].my_pos;
  /*
  Vector a_bit_forward;
  a_bit_forward.init_polar(0.5, targetdir);
  targetpos += a_bit_forward;
  */

  targetspeed = (ballpos - targetpos).norm()/summed_decay;

  //  DBLOG_MOV(0,"Should kick to "<< targetpos<<" kickspeed "<<targetspeed);
  Value speed1, speed2;

  // compute closest opponent to compute (virtual) state for kick command.
  Vector oppos;
  ANGLE opdir;
  int opdirage;
  WSpset pset= WSinfo::valid_opponents;
  pset.keep_and_sort_closest_players_to_point(1, ballpos);
  if ( pset.num ){
    oppos = pset[0]->pos;
    opdir = pset[0]->ang;
    opdirage = pset[0]->age_ang;
  }
  else{
    oppos = Vector(1000,1000); // outside pitch
    opdir = ANGLE(0);
    opdirage = 1000;
  }

  onetwostepkick->reset_state(); // reset state
  onetwostepkick->set_state(mypos,myvel,myang,ballpos,ballvel,oppos,opdir,opdirage);
  onetwostepkick->kick_to_pos_with_initial_vel(targetspeed,targetpos);
  onetwostepkick->get_vel(speed1,speed2);
  if(fabs(speed1-targetspeed)<=0.1){
    // everything's fine!
    onetwostepkick->get_cmd(simulation_table[0].cmd);
    steps = best_t;
    DBLOG_DRAW(0,C2D(targetpos.x,targetpos.y,.3,"orange"));          
    Vector tmp_ballpos =simulation_table[0].ball_pos;
    Vector tmp_ballvel;
    ANGLE kickdir = (targetpos -  tmp_ballpos).ARG();
    tmp_ballvel.init_polar(targetspeed, kickdir);

    if(are_intermediate_ballpositions_safe(tmp_ballpos, tmp_ballvel, steps) == true){
      DBLOG_MOV(0,"check KICK selfpass: SUCCESS: have the ball after "<<best_t<<" cycles, and  ballpos. safe!");
      return true;
    }
    else{
      DBLOG_MOV(0,"check kick selfpass: NO success: have the ball after "<<best_t<<" cycles, but   ballpos. NOT safe!");
      return false;
    }
  }
  //DBLOG_MOV(0,"determine correct kick: No one-step kick found! ");
  steps = best_t;
  return false;
}


bool Selfpass2::check_nokick_selfpass(Simtable *simulation_table, Vector & targetpos, int & steps, Vector &attacking_op, int & attacking_num, const Vector ballpos){
  int best_t =  -1;

  Value current_dist2border = Tools::min_distance_to_border(ballpos);

  int max_steps = 0;
  for(int t= MAX_STEPS-1; t>0; t--){  // start with t=1, since I know already that I have the ball at t= 0 (NOW!)
    if(simulation_table[t].I_have_ball== true){
      max_steps = t;
      break;
    }
  }

  //  DBLOG_MOV(0,"check nokick selfpass: Number of steps I have the ball without kicking: "<<max_steps);
    
  for(int t=1; t<=max_steps; t++){  // start with t=1, since I know already that I have the ball at t= 0 (NOW!)
    if(simulation_table[t].valid_at != WSinfo::ws->time){
      // entry not valid
      break;
    }
    if(simulation_table[t].op_steps2pos >= 0 && simulation_table[t].op_steps2pos <= t){ 
      // if opponent gets ball and he is faster or equally fast at position
      //DBLOG_MOV(0,"check nokick selfpass: Attackerpos: "<<attacking_op);
      break;
    }
    //    if((simulation_table[t].my_pos - simulation_table[t].ball_pos).norm() <= WSinfo::me->kick_radius - SAFETY_MARGIN){
    if(simulation_table[t].I_have_ball== true){
      // I have the ball at that time.
      if((Tools::min_distance_to_border(simulation_table[t].ball_pos) > MIN_DIST_2_BORDER) ||
	 (current_dist2border < MIN_DIST_2_BORDER && 
	  Tools::min_distance_to_border(simulation_table[t].ball_pos) > current_dist2border)){
	// position is either in pitch, or improves my current situation
	best_t = t;
      }  // ball is in pitch
    } // ball is kickable
  } // for all t


  if (best_t <0){ // no position is safe
    steps = best_t;
    return false;
  } 

  attacking_op = simulation_table[best_t].op_pos;
  attacking_num = simulation_table[best_t].op_num;
  //  DBLOG_MOV(0,"check with nokick: Attackerpos: "<<attacking_op);
  targetpos = simulation_table[best_t].my_pos;
  steps = best_t;
  DBLOG_MOV(0,"check nokick selfpass: SUCCESS: have the ball after "<<best_t<<" cycles, and  no op between!");
  return true;
}

bool Selfpass2::are_intermediate_ballpositions_safe(Vector tmp_ballpos, Vector tmp_ballvel, const int num_steps){
  // for all steps check, if ballposition is safe
  
  // check opponents;
  WSpset pset = WSinfo::valid_opponents; // enough, if I do it once.
  Vector resulting_pos;

  for(int t=0; t<=num_steps; t++){ 
    DBLOG_DRAW(0,C2D(tmp_ballpos.x,tmp_ballpos.y,.2,"grey"));          
    for (int idx = 0; idx < pset.num; idx++) {
      int op_steps = get_min_cycles2_pos(tmp_ballpos, pset[idx], t, resulting_pos);

      if(op_steps >=0){ // I found an opponent that gets to position in time
	DBLOG_MOV(0,"check intermediate ballpositions. opponent "<<pset[idx]->number<<" gets ball"
		  <<" in step "<<op_steps);
	DBLOG_DRAW(0,C2D(tmp_ballpos.x,tmp_ballpos.y,1.3,"black"));          
	return false;
      }
    } // for all ops idx
    tmp_ballpos += tmp_ballvel;
    tmp_ballvel *= ServerOptions::ball_decay;
  } // for all timesteps
  return true;
}