/* *  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: Meisam Vosoughpour * *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team. *  For more information please read README file.*/#include <cmath>#include <Line.h>#include <Degree.h>#include <Vector.h>#include <Logger.h>#include <Command.h>#include <Intercept.h>#include <BasicTurn.h>#include <AdvancedAgent.h>using namespace std;using namespace Degree;// class DribbleInterceptDribbleIntercept::DribbleIntercept(const WorldModel *worldModel):		AdvancedAction(worldModel){}void DribbleIntercept::execute(Form &form, const Library &library){	LOG << "DribbleIntercept::execute" << endl;	command = dashCommand;}float DribbleIntercept::getValue(const Library &library){	LOG << "DribbleIntercept::getValue" << endl;	Body simBody, nextBody;	Ball simBall, nextBall;		simBody = worldModel->getBody();	simBall = worldModel->getBall();	nextBody = worldModel->getBody();	nextBall = worldModel->getBall();	float kickableArea, virtualKickableArea;	float dashPower;	int dribbleGTBTime = 0;		kickableArea = worldModel->getBody().getSize() + 			worldModel->getBody().getKickableMargin() + worldModel->getBall().getSize();				virtualKickableArea = kickableArea + .2; // rate 1.06 for caution 	/*	float deltaAngleToBallVel;	deltaAngleToBallVel = worldModel->getBody()->getBodyDir - 					worldModel->getBall().getVel().getDirection();		deltaAngleToBallVel = Degree::normalizeAngle(deltaAngleToBallVel);*/			dashPower = 100;	if (worldModel->getBody().getStamina() < max(worldModel->getBody().getRecoverDecThr(),								 worldModel->getBody().getEffortDecThr()) *							 worldModel->getBody().getStaminaMax() + 150) // 150 for caution		dashPower = worldModel->getBody().getStaminaIncMax();		//dashCommand.dash(dashPower);	 	dashCommand = new DashCommand(AT_INTERCEPT, dashPower);	nextBody.simulateByAction(dashCommand);	nextBody.simulateByDynamics();	nextBall.simulateByDynamics(nextBody);			while (!(simBall.getBodyVec().getMagnitude() < kickableArea ||		   (simBall.getBodyVec().getMagnitude() < virtualKickableArea		   && nextBall.getBodyVec().getMagnitude() < virtualKickableArea) || 		   dribbleGTBTime > 30))	{		simBody.simulateByAction(dashCommand);		simBody.simulateByDynamics();		simBall.simulateByDynamics(simBody);			nextBody.simulateByAction(dashCommand);		nextBody.simulateByDynamics();		nextBall.simulateByDynamics(nextBody);					dribbleGTBTime++;	}		if (simBall.getBodyVec().getMagnitude() < kickableArea ||		   (simBall.getBodyVec().getMagnitude() < virtualKickableArea		   && nextBall.getBodyVec().getMagnitude() < virtualKickableArea))		return AD_ALWAYS_RUN_VALUE;	else	{		delete dashCommand;		return AD_MIN_VALUE;	}	}// class InterceptIntercept::Intercept(const WorldModel *worldModel): AdvancedAction(worldModel){}float Intercept::getValue(const Library &library){	LOG << "Intercept::getValue" << endl;	Body simBody;	Ball simBall;	Body waitBody;	Ball waitBall;	simBody = worldModel->getBody();	simBall = worldModel->getBall();	DashCommand tempDashCommand(AT_INTERCEPT, 0);	float bodyKickableArea;	float extraKickableForNoise;	float extraKickableForAngleNoise = 0;	int catchedBallCycles;	bool catchedBall;	int mainGoToBallTime = 0;	float dashPower;	float mainPositiveDashPower, mainNegativeDashPower;			mainPositiveDashPower = 100;	mainNegativeDashPower = -100;	if (worldModel->getBody().getStamina() < 1400)	{		mainPositiveDashPower = worldModel->getBody().getStaminaIncMax();		mainNegativeDashPower = -(worldModel->getBody().getStaminaIncMax() / 2);	}		Point defaultTurnPoint;	if (worldModel->getBody().getPos().getX() > 32)		defaultTurnPoint = Point(52, 0);	else		defaultTurnPoint = Point(worldModel->getBody().getPos().getX() + 10,				worldModel->getBody().getPos().getY());	bodyKickableArea = worldModel->getBody().getSize() + worldModel->getBody().getKickableMargin() +					   worldModel->getBall().getSize();	gtbPointsNumber = 0;	for (int i = 0; i < MAX_GTB_POINTS_NUM; i++)	{		gtbTime[i] = 999; 		minBallDistance[i] = bodyKickableArea;		extraGTBTime[i] = 0;		gtbPointWeight[i] = 0;		gtbCommand[i] = TurnToPoint(AT_INTERCEPT, defaultTurnPoint, worldModel->getBody()).getCommand();	}	////////NoAction	simBody = worldModel->getBody();	simBall = worldModel->getBall();	waitBody = simBody;	waitBall = simBall;			for (int i = 0; i <= WAIT_FOR_BALL_CYCLES; i++)	{//		float ExtraKickableForNoise = .15 * sqrt(MainGoToBallTime);		extraKickableForNoise = -.15;		if (waitBall.getBodyVec().getMagnitude() <= (bodyKickableArea + extraKickableForNoise))		{			gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 			extraGTBTime[gtbPointsNumber] = i;			mainGtbTime[gtbPointsNumber] = mainGoToBallTime + 								extraGTBTime[gtbPointsNumber];			gtbStatus[gtbPointsNumber] = 0;								gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);//*tof*/			gtbTime[gtbPointsNumber] -= 2;							minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyKickableArea - minBallDistance[gtbPointsNumber]) , (float).3) * 10; 					distToBegin[gtbPointsNumber] = hypot(waitBall.getPos().getX() - worldModel->getBall().getPos().getX(), waitBall.getPos().getY() - worldModel->getBall().getPos().getY());			catchedBall = 1;			gtbPointsNumber++;		}		waitBody.simulateByDynamics();		waitBall.simulateByDynamics(waitBody);	}	//	LOG << "MeisamGoToBall : gtbPointsNumberT& : " << gtbPointsNumber << endl;////////positiveDash		simBody = worldModel->getBody();	simBall = worldModel->getBall();	catchedBallCycles = 0;	catchedBall = 0;	mainGoToBallTime = 0;	dashPower = 100;	if (simBody.getStamina() < 1400)				dashPower = simBody.getStaminaIncMax();	tempDashCommand.setPower(dashPower);	waitBody = simBody;	waitBall = simBall;	while (catchedBallCycles <= 3 && mainGoToBallTime < MAX_POSITIVE_DASH_CYCLES)	{		mainGoToBallTime++;		//KOMAK		simBody.setStamina(simBody.getStamina() + simBody.getStaminaIncMax());		waitBody = simBody;		waitBall = simBall;				dashPower = 100;		if (simBody.getStamina() < 1400)			dashPower = simBody.getStaminaIncMax();		tempDashCommand.setPower(dashPower);		delete gtbCommand[gtbPointsNumber];		gtbCommand[gtbPointsNumber] = new DashCommand(AT_INTERCEPT, dashPower);		simBody.simulateByAction(dynamic_cast<Command *> (&tempDashCommand));		simBody.simulateByDynamics();		simBall.simulateByDynamics(simBody);//KOMAK		simBody.setStamina(simBody.getStamina() - DashPower);				for (int i = 0; i <= WAIT_FOR_BALL_CYCLES; i++)		{			extraKickableForNoise = worldModel->getBall().getBodyVec().getMagnitude() * .07;/////////////////////POSITIVE ANGLE NOISE						Line bodyAngleLine, ballVelLine;			Point interceptGTBPoint;			bodyAngleLine.setBySourceDir(Point(worldModel->getBody().getPos().getX(), worldModel->getBody().getPos().getY()), worldModel->getBody().getBodyDir());			ballVelLine.setBySourceDir(Point(worldModel->getBall().getPos().getX(), worldModel->getBall().getPos().getY()), worldModel->getBody().getVel().getDirection());			bodyAngleLine.getLineIntersect(ballVelLine, interceptGTBPoint);			Vector interceptGTBVector;			interceptGTBVector.setByPoints(Point(worldModel->getBody().getPos().getX(), worldModel->getBody().getPos().getY()), interceptGTBPoint);			if (fabs(normalizeAngle(interceptGTBVector.getDirection() - worldModel->getBody().getBodyDir())) < 2)			{				float deltaAngleToBallAngle;				deltaAngleToBallAngle = simBody.getBodyDir() - simBall.getVel().getDirection();				deltaAngleToBallAngle = normalizeAngle(deltaAngleToBallAngle);				if (interceptGTBVector.getMagnitude() < 8)				{					if (fabs(deltaAngleToBallAngle) < 90)						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) < 120)						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .2;					else if (fabs(deltaAngleToBallAngle) >= 120)						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .4;				}				else				{					if (fabs(deltaAngleToBallAngle) < 90)						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) < 120)						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) >= 120)						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;				}			}			else			{				extraKickableForAngleNoise = 0;				extraKickableForNoise /= 2;			}			if (waitBall.getBodyVec().getMagnitude() <= (bodyKickableArea + extraKickableForNoise + extraKickableForAngleNoise))			{				gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 				extraGTBTime[gtbPointsNumber] = i;								mainGtbTime[gtbPointsNumber] = mainGoToBallTime + 								extraGTBTime[gtbPointsNumber];				gtbStatus[gtbPointsNumber] = 1;													gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);///*TOF*/				gtbTime[gtbPointsNumber] -= 0;								minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyKickableArea - minBallDistance[gtbPointsNumber]) , (float).3) * 10; 						distToBegin[gtbPointsNumber] = hypot(waitBall.getPos().getX() - worldModel->getBall().getPos().getX(), waitBall.getPos().getY() - worldModel->getBall().getPos().getY());				delete gtbCommand[gtbPointsNumber];				gtbCommand[gtbPointsNumber] = new DashCommand(AT_INTERCEPT, mainPositiveDashPower);				catchedBall = 1;				gtbPointsNumber++;			}			waitBody.simulateByDynamics();			waitBall.simulateByDynamics(waitBody);		}				if (catchedBall) 				catchedBallCycles++;	}//	LOG << "MeisamGoToBall : gtbPointsNumberT? : " << gtbPointsNumber << endl;	//////////////////Negative Dash		simBody = worldModel->getBody();	simBall = worldModel->getBall();	catchedBallCycles = 0;	catchedBall = 0;	mainGoToBallTime = 0;	dashPower = -100;	if (simBody.getStamina() < 1400)		dashPower = -simBody.getStaminaIncMax() / 2.00;	tempDashCommand.setPower(-dashPower);	while (catchedBallCycles <= 3 && mainGoToBallTime < MAX_NEGATIVE_DASH_CYCLES)	{		mainGoToBallTime++;		//KOMAK		simBody.setStamina(simBody.getStamina() + simBody.getStaminaIncMax());		waitBody = simBody;		waitBall = simBall;				dashPower = -100;		if(simBody.getStamina() < 1400)			dashPower = -simBody.getStaminaIncMax() / 2.00;		tempDashCommand.setPower(dashPower);		delete gtbCommand[gtbPointsNumber];		gtbCommand[gtbPointsNumber] = new DashCommand(AT_INTERCEPT, dashPower);		simBody.simulateByAction(&tempDashCommand);		simBody.simulateByDynamics();		simBall.simulateByDynamics(simBody);//KOMAK		simBody.setStamina(simBody.getStamina() + dashPower * 2);////dar inja dashpower manfi ast.				for (int i = 0; i <= WAIT_FOR_BALL_CYCLES; i++)		{			extraKickableForNoise = worldModel->getBall().getBodyVec().getMagnitude() * .07;			float deltaAngleToBallAngle, negBodyAngle;			negBodyAngle = simBody.getBodyDir() + 180;			deltaAngleToBallAngle = negBodyAngle - simBall.getVel().getDirection();			deltaAngleToBallAngle = normalizeAngle(deltaAngleToBallAngle);/////////////////////NEGATIVE ANGLE NOISE						Line bodyAngleLine, ballVelLine;			Point interceptGTBPoint;			bodyAngleLine.setBySourceDir(Point(simBody.getPos().getX(), simBody.getPos().getY()), negBodyAngle);						ballVelLine.setBySourceDir(Point(simBall.getPos().getX(), simBall.getPos().getY()), worldModel->getBall().getVel().getDirection());			bodyAngleLine.getLineIntersect(ballVelLine, interceptGTBPoint);			Vector interceptGTBVector;			interceptGTBVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), interceptGTBPoint);			if (fabs(normalizeAngle(interceptGTBVector.getDirection() - negBodyAngle)) < 2)			{