generategoal.cc

2007年机器人足球世界杯3D仿真组亚军于07年底的源程序。该队还于2008年获得伊朗公开赛的亚军。

#include "kickto.ih"

rf<Behavior::Goal> KickTo::generateGoal(unsigned step, unsigned slot)
{
  rf<Goal> goal = new Goal();
  rf<OrNode> dis = goal->addDisjunct();
  rf<AndNode> con = dis->addConjunct();
  
  if (step == 1)
  {
    WorldModel& wm = WorldModel::getInstance();
    Vector3D ballPos = wm.getObjectPosition(Types::BALL).getMu();
    Vector3D goal1Pos = wm.getObjectPosition(Types::GOAL1THEM).getMu();
    Vector3D goal2Pos = wm.getObjectPosition(Types::GOAL2THEM).getMu();
    
    Vector3D goalPos = (goal1Pos + goal2Pos) / 2;
    Vector3D goalXYPos = goalPos;
    goalXYPos.setZ(0);
    Vector3D straight(1,0,0);
    
    double angle = straight.angle(goalXYPos);
    if (goalXYPos.getY() < 0)
      angle = -angle;
      
    Vector3D dir = goalPos - ballPos;

    _debugLevel4(ballPos << " " << dir);
    ballPos = ballPos - dir / dir.length() * 0.9;
    _debugLevel4(ballPos);
    Vector3D ballPosLeft;
    ballPosLeft.setX(dir.getY());
    ballPosLeft.setY(dir.getX());
    
    _debugLevel4(goal1Pos << " " << goal2Pos << " " << goalPos << " " << angle);
    
    Vector3D targetPos = ballPos + ballPosLeft.normalize() * .4;
    
/*    if (!Math::atSameSideOf(ballPos, targetPos, Vector3D(0,0,0)))
    {
      targetPos = ballPos + ballPosLeft.normalize() * 4;
      if (!Math::atSameSideOf(ballPos, Vector3D(0,0,0), targetPos))
        targetPos = ballPos - ballPosLeft.normalize() * 4;
    }
*/

    con->addVar("Pos", targetPos);
    con->addVar("Angle", angle, angle);
  }  
  
  // con->addVar("Var", 0, 1);
  
  return goal;
}