worldmodelpredict.cpp

根据 trilearn_base_sources-3.3 (一般称做UVA底层)修改的robocup球队代码,对某些模块进行了详细的注释(例如BasicPlayer,WorldMode模块),并且进行

  double      dDist  =line.getDistanceWithPoint( posTo );

  // get the angle to this point
  angTo = (posTo - posPred).getDirection();
  angTo = VecPosition::normalizeAngle( angTo - angBody );

  // determine whether we want to turn based on orthogonal distance 
  double dRatioTurn;
  if( pos.getDistanceTo(posTo) > 30.0 ) 
    dRatioTurn = 4.0;  
  if( pos.getDistanceTo(posTo) > 20.0 ) 
    dRatioTurn = 3.0;  
  else if( pos.getDistanceTo(posTo) > 10 ) 
    dRatioTurn = 2.0;
  else
    dRatioTurn = 0.90 ;

  AngDeg angTmp = angTo + (bMoveBack) ? 180 : 0;
  angTmp = VecPosition::normalizeAngle( angTmp );
  
  // turn when: 
  //  1. point lies outside our body range (forward and backwards)
  //  2. point lies outside distBack and behind us (forward move) 
  //  3. point lies outside distBack and in front of us backwards move)
  int    turn = 0;
  while( ( dDist > dRatioTurn*getMaximalKickDist( obj ) ||
          ( posPred.getDistanceTo( posTo ) > dDistBack && 
              ( ( fabs( angTo ) > 90 && bMoveBack == false  ) ||
                ( fabs( angTo ) < 90 && bMoveBack == true ) ) ) )
         && turn < 5 && fabs( angTmp ) > PS->getPlayerWhenToTurnAngle() )
  {

     ang = (posTo - posPred).getDirection() + (( bMoveBack == true )?180:0);
     ang = VecPosition::normalizeAngle( ang - angBody );
     soc = SoccerCommand(CMD_TURN,getAngleForTurn(ang,vel.getMagnitude(),obj));
     Log.log( 468, "angTo %f, dDist %f, ang %f %d angBody %f soc %f vel %f %f",
              angTo, dDist, ang, obj, angBody, soc.dAngle, vel.getMagnitude(),
              getInertiaMoment( obj ));
     if( bFirst == true )
       socFirst = soc;
     bFirst = false;
     predictStateAfterTurn(soc.dAngle, &pos, &vel, &angBody,&angNeck,obj,&sta);
     line = Line::makeLineFromPositionAndAngle( posPred, angBody );
     dDist = line.getDistanceWithPoint( posTo );
     angTo = (posTo - posPred).getDirection();
     angTo = VecPosition::normalizeAngle( angTo - angBody );
     turn++;
  }

  // if very close and have to turn a lot, it may be better to move with our
  // back to that point
  if( turn > 1 && iCycles < 4 && posPred.getDistanceTo( posTo ) < dDistBack &&
      bMoveBack == false)
  {
     angBody = ( angBodyIn == NULL ) ? getGlobalBodyAngle( obj ) : *angBodyIn;
     pos     = ( posIn     == NULL ) ? getGlobalPosition ( obj ) : *posIn;
     vel     = ( velIn     == NULL ) ? getGlobalVelocity ( obj ) : *velIn;
     ang = (posTo - posPred).getDirection() + 180;
     ang = VecPosition::normalizeAngle( ang - angBody );
     soc = SoccerCommand(CMD_TURN,getAngleForTurn(ang,vel.getMagnitude(),obj));
     predictStateAfterTurn( soc.dAngle,&pos,&vel,&angBody,&angNeck,obj,&sta);
     line = Line::makeLineFromPositionAndAngle( posPred, angBody );
     dDist = line.getDistanceWithPoint( posTo );
     if( dDist < 0.9*getMaximalKickDist( obj )  )
     {
       Log.log( 463, "turn around and intercept with back" );
       return soc;
     }
  }

  return socFirst;
}

/*! This method returns the command to move to a position, first it checks
    whether a turn is necessary. When this is the case, it performs the turn.
    Otherwise a dash command is generated to move in 'iCycles' cycles to
    the point 'posTo'. */
/*  这个方法返回移动到一个点需要的指令，首先它判断是否必须先转身。
    需要的话首先转身，否则冲刺向指定的点posTo。
    ObjectT obj : 预测跑动指令的物体
    VecPosition posTo : 预定要跑向的点
    int iCycles : 需要在多少周期跑动到指定的点  */
SoccerCommand WorldModel::predictCommandToMoveToPos( ObjectT obj, 
  VecPosition posTo, int iCycles, double dDistBack, 
  bool bMoveBack,VecPosition *posIn, VecPosition *velIn, AngDeg *angBodyIn)
{
  VecPosition   pos, vel;
  AngDeg        angBody;
  SoccerCommand soc;
  double        dPower;

  // fill in all values
  angBody = ( angBodyIn == NULL ) ? getGlobalBodyAngle( obj ) : *angBodyIn;
  pos     = ( posIn     == NULL ) ? getGlobalPosition ( obj ) : *posIn;
  vel     = ( velIn     == NULL ) ? getGlobalVelocity ( obj ) : *velIn;

  soc = predictCommandTurnTowards(obj, posTo, iCycles, dDistBack, bMoveBack,
                                       posIn, velIn,   angBodyIn);
  if( ! soc.isIllegal() )
    return soc;

  dPower = getPowerForDash( posTo-pos, angBody, vel,getAgentEffort(),iCycles );
  return SoccerCommand( CMD_DASH, dPower );
}

/*! 这个函数作为截球指令预测函数的子函数使用，当然也可以调用它预测其他球员截球需要的周期数。
    \param obj 需要预测截球周期的物体 Object
    \param posIntercept 返回最终计算得出的截球点
    \return 返回近似预测出的截球需要周期  */
int WorldModel::predictNrCyclesToInterceptBall( ObjectT obj,VecPosition *posIntercept )
{
  //定义用到的变量
  int i,iCycle,max_cycle=PS->getPlayerWhenToIntercept();
  VecPosition posTo,posFrom,posPred1,posPred2;
  AngRad rad1,rad2;
  Line line1(0,0,0);
  HeteroPlayerSettings hp_info;//异构球员的说明
  double max_speed,now_speed,acc_max,dist_max,tmp;//最大速度，当前速度，最大加速度，临时变量

  //初始化obj当前的点，球当前的点
  posFrom = getGlobalPosition(obj);
  posTo = getBallPos();

  //当已经处截球成功位置时，返回0
  if( posFrom.getDistanceTo(posTo) < getMaximalKickDist(obj) )
  {
    return 0;
  }
  //否则就需要计算截球的最小周期数

  //首先做好所有的准备

  //判断当前是不是对方球员带球,并且我们在对方球员的后面(如果成立的话，假设对方球员会向前带球)
  bool isThemControl = ( whoControlBall()==CONTROL_THEM && posTo.getX()<posFrom.getX());

  //记录截球点为无效
  posTo = VecPosition(UnknownDoubleValue,UnknownDoubleValue);
  //记录截球周期为一个最大值
  iCycle=100;

  //取得异构球员的最大速度和加速度
  hp_info=getHeteroInfoPlayer(obj);//取得异构球员参数说明
  acc_max=hp_info.dDashPowerRate*getAgentEffort()*SS->getMaxPower();//以当前体力利用率计算最大加速度
  max_speed=acc_max/(1.0-hp_info.dPlayerDecay);//最大速度
  max_speed=min(max_speed,hp_info.dPlayerSpeedMax);//因为最大速度有上限1.2，所以要校正
  //计算球员的当前速度
  now_speed=getGlobalVelocity(obj).getMagnitude();

  //循环查找截球的最小周期数
  posPred2=predictPosAfterNrCycles(OBJECT_BALL,1);
  i=1;
  while(i<=max_cycle)
  {
    //取得已经预测过的下个周期球的位置
    posPred1=posPred2;
    //如果球已经出界，直接返回
    if( isInField( posPred1 )==false && !isDeadBallUs() )
      return(iCycle);
    //在预测的基础上向左偏移1.0米，并向场地中间移动1.0米(用于绕过球员)
    if( isThemControl )
    {
      posPred1.addX(-1.0);
      posPred1.addY(-1.0*sign(posPred1.getY()));
    }
    //如果X大于一定范围的话，考虑将X移回，然后让预测点向球场中间移动
    if( posPred1.getX()<-45.0 )
    {
      posPred1.addX(0.8);
      posPred1.addY(-0.8*sign(posPred1.getY()));
    }
    //预测球下两个周期内的位置以及球员下个周期的位置（没有冲刺的情况下）
    posPred2=predictPosAfterNrCycles(OBJECT_BALL,i+1);
    posFrom=predictAgentPos(min(i,3),0);
    //计算球员到达指定位置需要的时间（近似值）
      //直接用距离除以最大速度
      iCycle=ceil(posFrom.getDistanceTo(posPred1)/max_speed);
      //根据现在的速度加上加速带来的额外周期数
      if(now_speed<max_speed/3.0)
      {
        iCycle++;
      }
      else if(now_speed<max_speed/6.0)
      {
        iCycle+=2;
      }
      //计算进行转身带来的额外周期数
      //首先计算两个角度：身体角度，球对于obj物体的相对角度
      rad1=getGlobalBodyAngle(obj);
      rad2=(posPred1-posFrom).getDirection();
      //制作一条和球员身体朝向相同的直线
      line1=Line::makeLineFromPositionAndAngle( posFrom,rad1);
      //如果直接跑动会导致距离球过远
      if( line1.getDistanceWithPoint(posPred1) > getMaximalKickDist(obj) )
      {
        //夹角的差
        rad1=fabs(rad2-rad1);
        //一个周期可以完成转身
        if( rad1<(180/(1.0+hp_info.dInertiaMoment * now_speed)) )
        {
          iCycle++;
        }
        else
        {
          rad1-=(180/(1.0+hp_info.dInertiaMoment * now_speed));
          if(rad1<180/(1.0+hp_info.dInertiaMoment * now_speed * hp_info.dPlayerDecay))
          {
            iCycle+=2;
          }
          else
          {
            iCycle+=3;
          }
        }
      }
    //当周期小于i 或者 球已经接近于停止了的时候
    if( iCycle<i || (posPred2-posPred1).getMagnitude()< 0.01 )
    {
      posTo=posPred1;
      iCycle=max(iCycle,i);
      break;
    }
    //否则i++继续循环
    i++;
  }
  //再次计算到下一个预测的球的位置，用以取得一个更加保险的截球点
  if( isThemControl )
  {
    posPred2.addX(-1.0);
    posPred2.addY(-1.0*sign(posPred1.getY()));
  }
  if( posPred2.getX()<-45.0 )
  {
    posPred2.addX(0.8);
    posPred2.addY(-0.8*sign(posPred1.getY()));
  }
  i=ceil(posFrom.getDistanceTo(posPred2)/max_speed);
  if(now_speed<(max_speed/3.0))
    i++;
  else if(now_speed<(max_speed/6.0))
    i+=2;
  rad1=getGlobalBodyAngle(obj);
  rad2=(posPred2-posFrom).getDirection();
  line1=Line::makeLineFromPositionAndAngle( posFrom,rad1);
  if( line1.getDistanceWithPoint(posPred1) > getMaximalKickDist(obj) )
  {
    rad1=fabs(rad2-rad1);
    if( rad1<(180/(1.0+hp_info.dInertiaMoment * now_speed)) )
    {
      i++;
    }
    else
    {
      rad1-=(180/(1.0+hp_info.dInertiaMoment * now_speed));
      if(rad1<180/(1.0+hp_info.dInertiaMoment * now_speed * hp_info.dPlayerDecay))
        i+=2;
      else
        i+=3;
    }
  }
  //如果计算得到的周期相同，则采用这个点作为新的截球目标点
  if(i<=iCycle)
  {
    posTo=posPred2;
    iCycle=i;
  }
  if(posIntercept!=NULL)
    *posIntercept=posTo;
  return(iCycle);
}

/*! 预测球员截球指令，根据 predictNrCyclesToInterceptBall 预测出的截球点进行活动。
    \param obj 需要预测截球指令的物体 Object
    \param soc 带入 interceptClose 方法返回的近距离截球指令
    \return 返回预测出的截球指令  */
SoccerCommand WorldModel::predictCommandToInterceptBall2(ObjectT obj,SoccerCommand socClose)
{
  //初始化变量
  SoccerCommand soc;
  VecPosition posTo(UnknownDoubleValue,UnknownDoubleValue);
  int iCycle=100;

  //计算新的截球点
  iCycle=predictNrCyclesToInterceptBall(obj,&posTo);
  //输出截球参数
  //cout<<"Cycle:"<<iCycle<<" \tposTo:";
  //posTo.show();

  //根据不同的情况判断是用什么指令
  //如果是在三个周期以内可以截球，而且socClose不是无效的
  if( iCycle <= 3 && ! socClose.isIllegal() )
  {
    Log.log( 463, "do close intercept" );
    soc = socClose;
  }
  //本来就已经处于截球成功状态
  else if( iCycle == 0 )
  {
    Log.log( 463, "intercept: do not move already close" );
    soc = SoccerCommand( CMD_ILLEGAL );
  }
  //否则
  else
  {
    Log.log( 463, "intercept: move to (%f,%f)", posTo.getX(),posTo.getY());