#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;}