behave.c

robocup 卡梅隆99源代码！本人没怎么看明白！望大家一起研究

/* -*- Mode: C++ -*- */

/* behave.C
 * CMUnited99 (soccer client for Robocup99)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1999 Peter Stone
 *
 * CMUnited-99 was created by Peter Stone, Patrick Riley, and Manuela Veloso
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 * For more information, please see http://www.cs.cmu.edu/~robosoccer/
 */


#include <fstream.h>
#include "behave.h"
#include "Memory.h"
#include "client.h"
#include "test.h"
#include "kick.h"
#include "dribble.h"

#define DebugMark(x) 

#ifdef DEBUG_OUTPUT
#define DebugBlock(x) x
#else
#define DebugBlock(x) 
#endif

//#define MAKE_DEMO
//#define TEST_PETER
//#define TEST_PAT
//#define TEST_PAT2

void behave(){
  
  /* Put any one of these test functions in -- but only one at a time */

  /* Start one player -- move the ball around the field */
  //face_ball();

  /* Start one player, press kickoff -- it will kick to the goal */
  test_go_to_ball();

  /* Start one player, press kickoff -- it will dribble to the goal */
  //test_dribble_to_goal();

  /* Start 2 players on different teams, press 'drop ball' near one player */
  //test_1v1();

  /* Start 2 players on different teams, press 'drop ball' near one player */
  //test_volley();

  /* These are to print out the positions of objects */
  //test_print_ball();
  //test_print_positions();

  /* Start 3 players on each team and press 'drop ball' in the middle of the field */
  // Rectangle *rect = new Rectangle(Vector(0,0),Vector(30,30));
  // test_random_movement_in_rectangle(rect);
  // delete rect;

  /* make sure that the option save_action_log_level is >= 200
     The player will generate a file
     Logfiles/<team_name><num>-<side>-actions.log which records state
     information and action choices. This logfile is pure text. It can
     be viewed more easily using the modified logplayer which is
     available with the CMUnited99 support code. This is part of a
     system we call Layered Extropsection. Please see the web page
     indicated above for more details */
  //test_log_action();
  
  /* Look in test.[Ch] for more interesting beahviors to play with */
}




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

ActionQueueRes scan_field_with_body()
{
  Mem->LogAction3(40,"scan_field_with_body (%d)",Mem->TimeToTurnForScan());
  if ( Mem->TimeToTurnForScan() ){
    turn(Mem->MyViewAngle() * 2 - Mem->CP_scan_overlap_angle);
    return AQ_ActionQueued;
  }
  else 
    return AQ_ActionNotQueued;
}

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

void turn_neck_to_relative_angle(AngleDeg ang)
{
  turn_neck(GetNormalizeAngleDeg(ang - Mem->MyNeckRelAng()));
}

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

void scan_field_with_neck()
{
  Mem->LogAction3(40,"scan_field_with_neck (%d)",Mem->TimeToTurnForScan());
  if ( Mem->TimeToTurnForScan() ){
    if ( Mem->MyNeckRelAng() >= Mem->SP_max_neck_angle-1 ) /* take into account reporting error */
      turn_neck_to_relative_angle(Mem->SP_min_neck_angle);
    else 
      turn_neck(Mem->LimitTurnNeckAngle(Mem->MyViewAngle()*2 - Mem->CP_scan_overlap_angle));
  }
}

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

ActionQueueRes face_only_body_to_point(Vector point)
{
  /* don't turn neck */
  Mem->LogAction4(30,"facing only body to point (%.1f %.1f)",point.x,point.y);

  /* shouldn't actually have queued actions at this point */
  AngleDeg target_rel_ang = Mem->PredictedPointRelAngFromBodyWithQueuedActions(point);

  if ( fabs(target_rel_ang) < 1 ){
    Mem->LogAction2(40,"Already close enough");
    return AQ_ActionNotQueued;
  }
  turn( target_rel_ang );
  return AQ_ActionQueued;
}

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

void face_only_neck_to_point(Vector point)
{
  /* don't turn body */
  Mem->LogAction4(30,"facing only neck to point (%.1f %.1f)",point.x,point.y);

  AngleDeg target_rel_ang = Mem->PredictedPointRelAngFromBodyWithQueuedActions(point);

  if ( fabs(GetNormalizeAngleDeg(Mem->MyNeckRelAng() - target_rel_ang)) < 1 ){
    Mem->LogAction2(40,"Already close enough");
    return;
  }

  if ( Mem->CanSeeAngleFromBodyWithNeck(target_rel_ang) ){
    turn_neck( Mem->LimitTurnNeckAngle(target_rel_ang - Mem->MyNeckRelAng()) );
  }
  else
    Mem->LogAction5(30,"can't face point (%.1f %.1f) with only neck (%.1f)",
		    point.x,point.y, target_rel_ang);
}

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

ActionQueueRes face_neck_to_point(Vector point)
{
  /* face_neck can turn body if needed */
  Mem->LogAction4(30,"facing neck to point (%.1f %.1f)",point.x,point.y);

  AngleDeg target_rel_ang = Mem->PredictedPointRelAngFromBodyWithQueuedActions(point);

  if ( fabs(GetNormalizeAngleDeg(Mem->MyNeckRelAng() - target_rel_ang)) < 1 ){
    Mem->LogAction2(40,"Already close enough");
    return AQ_ActionNotQueued;
  }

  if ( Mem->CanFaceAngleFromBodyWithNeck(target_rel_ang) ){
    Mem->LogAction2(35,"can face with neck");
    turn_neck_to_relative_angle(target_rel_ang);
    return AQ_ActionNotQueued;
  }

  /* If can't do it with just neck, turn body as much as needed to face directly */
  AngleDeg max_turn = Mem->MaxEffectiveTurn();
  if ( fabs(target_rel_ang) < max_turn ) {
    Mem->LogAction2(35,"can't face with neck, can with body");
    turn(target_rel_ang);
    turn_neck_to_relative_angle(0);
    return AQ_ActionQueued;
  }

  Mem->LogAction2(35,"can't face with neck or body alone, turning both");  
  turn(target_rel_ang);
  target_rel_ang -= max_turn;  /* The neck target_ang */

  if ( target_rel_ang < Mem->SP_min_neck_angle ){
    Mem->LogAction2(40,"couldn't face all the way");  
    turn_neck_to_relative_angle(Mem->SP_min_neck_angle);
  }
  else if ( target_rel_ang > Mem->SP_max_neck_angle ){
    Mem->LogAction2(40,"couldn't face all the way");  
    turn_neck_to_relative_angle(Mem->SP_max_neck_angle);
  }
  else 
    turn_neck_to_relative_angle(target_rel_ang);

  return AQ_ActionQueued;
}

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

ActionQueueRes face_neck_and_body_to_point(Vector point)
{
  /* face_neck_and_body will turn both as much as possible to the point */
  Mem->LogAction4(30,"facing neck and body to point (%.1f %.1f)",point.x,point.y);

  AngleDeg max_turn = Mem->MaxEffectiveTurn();
  AngleDeg target_rel_ang = Mem->PredictedPointRelAngFromBodyWithQueuedActions(point);

  if ( fabs(GetNormalizeAngleDeg(Mem->MyNeckRelAng() - target_rel_ang)) < 1 && fabs(target_rel_ang) < 1 ){
    Mem->LogAction2(40,"Already close enough");
    return AQ_ActionNotQueued;
  }

  if ( fabs(target_rel_ang) < max_turn ) {
    Mem->LogAction2(35,"Can get both neck and body there");
    /* Can get both neck and body there */
    face_only_body_to_point(point);
    turn_neck_to_relative_angle(0);
    return AQ_ActionQueued;
  }

  /* Turn body as much as possible and try to get neck there */
  return face_neck_to_point(point);
}

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

ActionQueueRes face_only_body_to_player(char side, Unum num)
{
  if ( Mem->PlayerPositionValid(side, num) ){
    return face_only_body_to_point(Mem->PlayerAbsolutePosition(side,num));
  }
  else
    return scan_field_with_body();
}

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

void face_only_neck_to_player(char side, Unum num)
{
  if ( Mem->PlayerPositionValid(side,num) ){
    face_only_neck_to_point(Mem->PlayerAbsolutePosition(side,num));
  }
  else
    scan_field_with_neck();
}

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

ActionQueueRes face_neck_to_player(char side, Unum num)
{
  if ( Mem->PlayerPositionValid(side, num) ){
    return face_neck_to_point(Mem->PlayerAbsolutePosition(side,num));
  }
  else
    return scan_field_with_body();
}

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

ActionQueueRes face_neck_and_body_to_player(char side, Unum num)
{
  if ( Mem->PlayerPositionValid(side, num) ){
    return face_neck_and_body_to_point(Mem->PlayerAbsolutePosition(side,num));
  }
  else
    return scan_field_with_body();
}

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

ActionQueueRes face_only_body_to_opponent(Unum opponent){ 
  Mem->LogAction3(30,"facing only body to opponent %d",opponent); 
  return face_only_body_to_player(Mem->TheirSide, opponent);
}

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

void           face_only_neck_to_opponent(Unum opponent){
  Mem->LogAction3(30,"facing only neck to opponent %d",opponent);
  face_only_neck_to_player(Mem->TheirSide, opponent);
}

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

ActionQueueRes face_neck_to_opponent(Unum opponent){
  Mem->LogAction3(30,"facing neck to opponent %d",opponent);
  return face_neck_to_player(Mem->TheirSide, opponent);
}

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

ActionQueueRes face_neck_and_body_to_opponent(Unum opponent){
  Mem->LogAction3(30,"facing neck and body to opponent %d",opponent);
  return face_neck_and_body_to_player(Mem->TheirSide, opponent);
}

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

ActionQueueRes face_only_body_to_teammate(Unum teammate){ 
  Mem->LogAction3(30,"facing only body to teammate %d",teammate); 
  return face_only_body_to_player(Mem->MySide, teammate);
}

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

void           face_only_neck_to_teammate(Unum teammate){
  Mem->LogAction3(30,"facing only neck to teammate %d",teammate);
  face_only_neck_to_player(Mem->MySide, teammate);
}

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

ActionQueueRes face_neck_to_teammate(Unum teammate){
  Mem->LogAction3(30,"facing neck to teammate %d",teammate);
  return face_neck_to_player(Mem->MySide, teammate);
}

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

ActionQueueRes face_neck_and_body_to_teammate(Unum teammate){
  Mem->LogAction3(30,"facing neck and body to teammate %d",teammate);
  return face_neck_and_body_to_player(Mem->MySide, teammate);
}

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

ActionQueueRes face_only_body_to_ball()
{
  Mem->LogAction2(30,"facing body to ball");
  if ( Mem->BallPositionValid() ) {
    return face_only_body_to_point(Mem->BallPredictedPositionWithQueuedActions());
  }
  else 
    return scan_field_with_body();
}

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

void face_only_neck_to_ball()
{
  Mem->LogAction2(30,"facing only neck to ball");
  if ( Mem->BallPositionValid() ) {
    face_only_neck_to_point(Mem->BallPredictedPositionWithQueuedActions());
  }
  else 
    scan_field_with_neck();
}

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

ActionQueueRes face_neck_to_ball()
{
  Mem->LogAction2(30,"facing neck to ball");
  if ( Mem->BallPositionValid() ) {
    return face_neck_to_point(Mem->BallPredictedPositionWithQueuedActions());
  }
  else