getcapability.cc

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

#include "guardgoal.ih"

Behavior::ConfidenceInterval GuardGoal::getCapability(rf<State> s, rf<Goal> g)
{
  WorldModel& wm = WorldModel::getInstance();

  if (d_curStep == 1)
  {
    // If we arent turned right in step 1 we cant do it
    Vector3D goalLine = wm.getObjectPosition(Types::GOAL1US).getMu() - wm.getObjectPosition(Types::GOAL2US).getMu();
    goalLine.setZ(0);
    Vector3D straight(1, 0, 0);
    double alpha = straight.angle(Math::calcPerpend(goalLine));
    if (fabs(alpha) > .1 * M_PI)
    {
      //cerr << "Turn!" << endl;
      return ConfidenceInterval(0.0, 0.0);
    }
  }
  
  if (fabs(WorldModel::getInstance().getDistToField()) < 1.5)
  {
    _debugLevel1("Too close to floor");
    return ConfidenceInterval(0.0, 0.0);
  }
  
  /*if (d_ballEndPos.length() < 0.2)
  {
    return ConfidenceInterval(0.3, 0.2);
  }
  
  if (d_ballEndPos.length() > 2)
    return ConfidenceInterval(0.6, 0.2);*/

  return ConfidenceInterval(0.5, 0.2);
}