/* *  Copyright 2002-2005, Mersad Team, Allameh Helli High School (NODET). * *  This program is free software, you can redistribute it and/or modify *  it under the terms of the GNU General Public License as published by *  the Free Software Foundation. * *  This program is distributed in the hope that it will be useful, *  but WITHOUT ANY WARRANTY; without even the implied warranty of *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *  GNU Library General Public License for more details. * *  This file is created by: Ahmad Boorghany * *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team. *  For more information please read README file.*/#include <cmath>#include <Types.h>#include <Logger.h>#include <Degree.h>#include <BasicDash.h>#include <PassPlan.h>#include <CrossPlan.h>#include <ShootPlan.h>#include <DribblePlan.h>#include <InterceptPlan.h>#include <AdvancedAgent.h>#include <SRPDribblePlan.h>#include <cassert>#include <Basics.h>using namespace std;using namespace Basics;CrossPlan::CrossPlan(const WorldModel *wm, Form &form, const Library &library):		Plan("Cross", wm, form, library){	fabric = true;	intercepting = false;	srpPlan = NULL;	passPlan = NULL;	shootPlan = NULL;	dribblePlan = NULL;	lastTurnTime = 0;}CrossPlan::~CrossPlan(){}void CrossPlan::decide(){	LOG << "CrossPlan::decide" << endl;	fabric = false;	if (wm->isBallKickable() || intercepting)	{		form.sayForm.planStatus = 3;		form.sayForm.doRoutinSay = true;	}LOG << "11" << endl;	Plan::decide();LOG << "12" << endl;	if (wm->isBallKickable())	{LOG << "13" << endl;		startPlan(selectedPlan, 0);	}	else	{/*		Point offensePoint =				library.positioning.getHomePoint(wm->getBody().getUniNum());		Command *command = library.positioning.getPositioningCommand(offensePoint);		command->setCreator(AT_OFFENSE);*/LOG << "14" << endl;		if (intercepting){LOG << "15" << endl;			startPlan(new InterceptPlan(wm, form, library));}		else		{LOG << "16" << endl;			form.sayForm.attentionNum = wm->getPlanStatusSender();			Point homePoint = library.positioning.getHomePoint(wm->getBody().getUniNum());			LOG << "\thomePoint: " << homePoint << endl;			LOG << "\tDist to Home: " << wm->getBody().getDistance(homePoint) << endl;			//			if (wm->getBall().getPos().getX() > 35 /*&&//				wm->getBody().getDistance(homePoint) < 0.5*/) // TEMP//				GlobalPositioning();			LOG << "\tDeltaY: " << homePoint.y - wm->getBody().getPos().getY() << endl;			LOG << "\tFrom last turn: " << wm->getCurTime() - lastTurnTime << endl;			if (lastTurnTime == 0 ||				(fabs(homePoint.y - wm->getBody().getPos().getY()) > 6.0 &&				 wm->getCurTime() - lastTurnTime > 5))			{ // We should turnLOG << "17" << endl;				Vector target;				target.setByPoints((Point)wm->getBody().getPos(),						Point(40,homePoint.y));				LOG << "\ttarget: " << target << endl;				LOG << "\tBest turn Angle: " << fabs(Degree::getDeltaAngle(wm->getBody().getBodyDir(),						target.getDirection())) << endl;				LOG << "\tDelta Angle to front: " << fabs(wm->getBody().getBodyDir()) << endl;				LOG << "\tisBetween: " << Degree::isBetween(0, target.getDirection(), wm->getBody().getBodyDir()) << endl;				if (lastTurnTime == 0 && // It is the start.					fabs(Degree::getDeltaAngle(wm->getBody().getBodyDir(),							target.getDirection())) < 7.5 &&					(Degree::isBetween(0, target.getDirection(), wm->getBody().getBodyDir()) ||					 fabs(wm->getBody().getBodyDir()) < 3))				{ // Start going straight					lastTurnTime = wm->getCurTime();				}				else				{LOG << "18" << endl;					Command *command = DashNormalGotoPoint(AT_OFFENSE,							Point(40,homePoint.y), 2, wm->getBody()).getCommand();					if (!dynamic_cast<TurnCommand *>(command) || // for multi turns						!fabs(dynamic_cast<TurnCommand *>(command)->getDirection()) == 180)						lastTurnTime = wm->getCurTime();					throw command;				}			}			// We should go straightLOG << "19" << endl;			Vector straight(Point(20, 0));			straight.rotate(wm->getBody().getBodyDir());			Command *command = DashNormalGotoPoint(AT_OFFENSE,					wm->getBody().getPos() + straight, 1, wm->getBody()).getCommand();			if (wm->getBody().getPos().getX() > wm->getOppOffsideLine(true) + 2)			{ // nearly I stop.				float dashPower = fmin(						dynamic_cast<DashCommand *>(command)->getPower(), 5);				dynamic_cast<DashCommand *>(command)->setPower(dashPower);			}			else if (wm->getBody().getPos().getX() > wm->getOppOffsideLine(true) - 1.5)			{ // I coorse slowly.				float dashPower = fmin(						dynamic_cast<DashCommand *>(command)->getPower(), 20);				dynamic_cast<DashCommand *>(command)->setPower(dashPower);			}			throw command;		}	}	LOG << "CrossPlan Now Kill Program" << endl;	assert(0);}double CrossPlan::successRate(){	LOG << "CrossPlan::successRate" << endl;	LOG << "FABRIC:" << fabric << endl;	if (wm->isBallKickable())	{		intercepting = false;		// Check for shoot		if (shootPlan) delete shootPlan;		shootPlan = new ShootPlan(wm, form, library);		float val = shootPlan->successRate();		if (val > 0.75) // ?		{			LOG << "\t --> Shoot executed: " << val << endl;			selectedPlan = shootPlan;			shootPlan = NULL;			return 1.0;		}		if (wm->getBody().getPos().getX() >				config["Agent"]["PlanningSystem"]["GoalPlanX"].asFloat())		{			LOG << "Coorse cancled... Taghdeem be goalPlan" << endl;			return 0.0;		}		if (fabric == true)		{			float oppOffsideLine = wm->getOppLocalOffLine();			if (oppOffsideLine == -0xFFFF)				oppOffsideLine = wm->getOppOffsideLine();			if (wm->getBall().getPos().getX() > oppOffsideLine - 6 &&				wm->getBall().getPos().getX() > 15)			{				if (srpPlan) delete srpPlan;				srpPlan = new SRPDribblePlan(wm, form, library);				float val = srpPlan->successRate();				LOG << "\tSRPDribble Success Rate: " << val << endl;				if (val > 0.25)				{					LOG << "---> Coorse started." << endl;					selectedPlan = srpPlan;					srpPlan = NULL;					intercepting = true;					return 1.0;				}			}			return 0.0;		}		else		{			// Check for pass			if (dribblePlan) delete dribblePlan;			dribblePlan = new DribblePlan(wm, form, library);			float val = dribblePlan->successRate();			LOG << "\tDribble Success Rate:" << val << endl;			if (val < 0.25) // ?			{				if (passPlan) delete passPlan;				passPlan = new PassPlan(wm, form, library, BPM_ALL, BPN_ALL);				float val = passPlan->successRate();				LOG << "\tPass success rate: " << val << endl;				if (val > 0.23) // ?				{					LOG << "\t --> Pass executed." << endl;					selectedPlan = passPlan;					passPlan = NULL;					return 1.0;				}			}			// Check for SRPDribble			if (srpPlan) delete srpPlan;			srpPlan = new SRPDribblePlan(wm, form, library);			val = srpPlan->successRate();			LOG << "\tSRPDribble Success Rate:" << val << endl;			if (val > 0.18)			{				LOG << "---> Coorse continued." << endl;				selectedPlan = srpPlan;				srpPlan = NULL;				intercepting = true;				return 1.0;			}			else			{				float oppOffsideLine = wm->getOppLocalOffLine();				if (oppOffsideLine == -0xFFFF ||					oppOffsideLine < wm->getBall().getPos().getX() + 1.5)				{ // I'll check if I can do Dribble instead SRPDribble					if (dribblePlan) delete dribblePlan;					dribblePlan = new DribblePlan(wm, form, library);					float val = dribblePlan->successRate();					if (val > 0.2) // ?					{						selectedPlan = dribblePlan;						dribblePlan = NULL;						LOG << "---> Coorse continued as Dribble." << endl;						intercepting = true;						return 1.0;					}				}			}			LOG << "---> Coorse canceled." << endl;			return 0.0;		}	}	else	{		if (intercepting) // who is intercepting.		{			if (wm->getCurInterCalculate().getCheckedListNum(wm->getBody()) == NOVALUE)				return 0.0; // I failed the coorse.			return 1.0;		}		else // who is going to penalty area.			if (!isInPlayers(wm->getBody().getUniNum(), "AB") && // A & B are independent				wm->getHearedPlanStatus() == 3 &&				wm->getBody().getPos().getX() <					config["Agent"]["PlanningSystem"]["GoalPlanX"].asFloat())				return 1.0;	}	return 0.0;}bool CrossPlan::isFinished(){	if (!isInPlayers(wm->getBody().getUniNum(), "234AB"))		return true;	return false;}void CrossPlan::GlobalPositioning(){	LOG << "CrossPlan::GlobalPositioning" << endl;	Point offensePoint =			library.positioning.getHomePoint(wm->getBody().getUniNum());	LOG << "\toffensePoint: " << offensePoint << endl;	Command *command = library.positioning.getPositioningCommand(offensePoint);	command->setCreator(AT_OFFENSE);		throw command;}void CrossPlan::resetAfterCopy(){	while (!subPlans.empty())		subPlans.pop();	srpPlan = NULL;	passPlan = NULL;	shootPlan = NULL;	dribblePlan = NULL;	selectedPlan = NULL;}