generategoal.cc

来自「2007年机器人足球世界杯3D仿真组亚军于07年底的源程序。该队还于2008年获」· CC 代码 · 共 33 行

#include "walkto.ih"

rf<Behavior::Goal> WalkTo::generateGoal(unsigned step, unsigned slot)
{
  Vector3D pos, straight(1,0,0);
  double angleToPos;
  
  rf<Goal> goal = new Goal();
  rf<OrNode> dis = goal->addDisjunct();
  rf<AndNode> con = dis->addConjunct();
      
  switch (step)
  {
    case 0:
      pos = d_goal->getVector("Pos");
      pos.setZ(0);

      angleToPos = pos.angle(straight) / M_PI * 180;
      if (pos.getY() < 0)
        angleToPos = -angleToPos;
        
      _debugLevel4(ident() << " " << pos << " " << angleToPos);
      
      con->addVar("Angle", angleToPos, angleToPos);    
      return goal;
      
    case 1:
      return d_goal;
  }
  
  return 0;
}