/* *  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 <BasicDash.h>#include <Line.h>#include <Vector.h>#include <Degree.h>#include <Logger.h>#include <Command.h>#include <BasicTurn.h>#include <GoalieIntercept.h>#include <AdvancedAgent.h>using namespace std;// class GoalieCatchGoalieCatch::GoalieCatch(const WorldModel *worldModel):		AdvancedAction(worldModel){}void GoalieCatch::execute(Form &form, const Library &library){	if (catchableFlag == true)		command = new CatchCommand(AT_CATCH,				worldModel->getBall().getBodyVec().getDirection());	else		command = new KickCommand(AT_GOALIE_INTERCEPT, 100, 0);		}float GoalieCatch::getValue(const Library &library){	if (worldModel->getBall().getPos().getX() < -36.4 &&		worldModel->getBody().getPos().getX() < -36.4 &&		fabs(worldModel->getBall().getPos().getY()) < 19.6 &&		fabs(worldModel->getBody().getPos().getY()) < 19.6)	{		if (library.lastBodyCycleCommand->getCreator() == AT_PASS)		{			catchableFlag = false;			return AD_MIN_VALUE;		}		if (worldModel->isBallCatchable())		{			catchableFlag = true;			return AD_ALWAYS_RUN_VALUE;		}		}/*	else if (worldModel->isBallKickable())	{		catchableFlag = false;			return AD_ALWAYS_RUN_VALUE;	}*/	// I do not want to do any action.	catchableFlag = false;		return AD_MIN_VALUE;}// class GoalieInterceptGoalieIntercept::GoalieIntercept(const WorldModel *worldModel): AdvancedAction(worldModel){}float GoalieIntercept::getValue(const Library &library){	LOG << "GoalieIntercept::getValue" << endl;	Body simBody;	Ball simBall;	Body waitBody;	Ball waitBall;	simBody = worldModel->getBody();	simBall = worldModel->getBall();	DashCommand tempDashCommand(AT_GOALIE_INTERCEPT, 0);	float bodyCatchableArea;	float extraCatchableForNoise;	float extraCatchableForAngleNoise = 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);	}		bodyCatchableArea = hypot(worldModel->getBody().getCatchableAreaL(), 						worldModel->getBody().getCatchableAreaW() / 2.00); 	gtbPointsNumber = 0;	for (int i = 0; i < GOALIE_MAX_GTB_POINTS_NUM; i++)	{		gtbTime[i] = 999; 		minBallDistance[i] = bodyCatchableArea;		extraGTBTime[i] = 0;		gtbPointWeight[i] = 0;		gtbCommand[i] = NULL;//TurnToPoint(Point(50, 0), worldModel->getBody()).getCommand();	}	////////NoAction	simBody = worldModel->getBody();	simBall = worldModel->getBall();	waitBody = simBody;	waitBall = simBall;			for (int i = 0; i <= GOALIE_WAIT_FOR_BALL_CYCLES; i++)	{//		float ExtraCatchableForNoise = .15 * sqrt(MainGoToBallTime);		extraCatchableForNoise = -.15;		if (waitBall.getBodyVec().getMagnitude() <= (bodyCatchableArea + extraCatchableForNoise))		{				gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 			extraGTBTime[gtbPointsNumber] = i;			gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);//*tof*/			gtbTime[gtbPointsNumber] -= 2;							minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyCatchableArea - minBallDistance[gtbPointsNumber]) , (float).3) * 10; 					distToBegin[gtbPointsNumber] = hypot(waitBall.getPos().getX() - worldModel->getBall().getPos().getX(), waitBall.getPos().getY() - worldModel->getBall().getPos().getY());			gtbPointWeight[gtbPointsNumber] -= 1000;			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 < GOALIE_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_GOALIE_INTERCEPT, dashPower);		simBody.simulateByAction(dynamic_cast<Command *> (&tempDashCommand));		simBody.simulateByDynamics();		simBall.simulateByDynamics(simBody);//KOMAK		simBody.setStamina(simBody.getStamina() - DashPower);				for (int i = 0; i <= GOALIE_WAIT_FOR_BALL_CYCLES; i++)		{			extraCatchableForNoise = worldModel->getBall().getBodyVec().getMagnitude() * .12;			extraCatchableForNoise += .3;/////////////////////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(Degree::normalizeAngle(interceptGTBVector.getDirection() - worldModel->getBody().getBodyDir())) < 2)			{				float deltaAngleToBallAngle;				deltaAngleToBallAngle = simBody.getBodyDir() - simBall.getVel().getDirection();				deltaAngleToBallAngle = Degree::normalizeAngle(deltaAngleToBallAngle);				if (interceptGTBVector.getMagnitude() < 8)				{					if (fabs(deltaAngleToBallAngle) < 90)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) < 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .2;					else if (fabs(deltaAngleToBallAngle) >= 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .4;				}				else				{					if (fabs(deltaAngleToBallAngle) < 90)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) < 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) >= 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;				}			}			else			{				extraCatchableForAngleNoise = 0;				extraCatchableForNoise /= 2;			}			if (waitBall.getBodyVec().getMagnitude() <= (bodyCatchableArea + extraCatchableForNoise + extraCatchableForAngleNoise))			{				gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 				extraGTBTime[gtbPointsNumber] = i;				gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);///*TOF*/				gtbTime[gtbPointsNumber] -= 0;								minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyCatchableArea - 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_GOALIE_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 < GOALIE_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_GOALIE_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 <= GOALIE_WAIT_FOR_BALL_CYCLES; i++)		{			extraCatchableForNoise = worldModel->getBall().getBodyVec().getMagnitude() * .12;			extraCatchableForNoise += .3;			float deltaAngleToBallAngle, negBodyAngle;			negBodyAngle = simBody.getBodyDir() + 180;			deltaAngleToBallAngle = negBodyAngle - simBall.getVel().getDirection();			deltaAngleToBallAngle = Degree::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(Degree::normalizeAngle(interceptGTBVector.getDirection() - negBodyAngle)) < 2)			{				if (interceptGTBVector.getMagnitude() < 8)				{					if (fabs(deltaAngleToBallAngle) < 90)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) < 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .2;					else if (fabs(deltaAngleToBallAngle) >= 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .4;				}				else				{					if (fabs(deltaAngleToBallAngle) < 90)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if (fabs(deltaAngleToBallAngle) < 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;					else if(fabs(deltaAngleToBallAngle) >= 120)						extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;				}			}			else			{				extraCatchableForAngleNoise = 0;				extraCatchableForNoise /= 2.00;			}		/////////////////////NEGATIVE ANGLE NOISE