/* *  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 <Logger.h>#include <Defense.h>#include <Degree.h>#include <BasicTurn.h>#include <BasicDash.h>#include <AdvancedAgent.h>using namespace std;Defense::Defense(const WorldModel *worldModel): AdvancedAction(worldModel){}float Defense::getValue(const Library &library){	LOG << "Defense::getValue" << endl;    defensePoint = library.positioning.getHomePoint(worldModel->getBody().getUniNum());//	LOG << "defensePoint1 : " << defensePoint << endl;    float senseX = 0, senseY = 0; 	    if(worldModel->getBody().getPostNum() == 5 || 		worldModel->getBody().getPostNum() == 2)	    {		senseX = 5;		senseY = 5;		if (worldModel->getBall().getPos().getX() > -20)		{			senseX = 5;			senseY = 2.5;		}	    }    	if(worldModel->getBody().getPostNum() == 4 || 		worldModel->getBody().getPostNum() == 3)	    {		senseX = 4;		senseY = 1.8;				if (worldModel->getBall().getPos().getX() > -20)		{			senseX = 4;			senseY = 1.2;		}    }				    if(worldModel->getBody().getPostNum() == 7 ||	   		worldModel->getBody().getPostNum() == 6)	    {		senseX = 13;		senseY = 13;		if (worldModel->getPlayMode() == PM_KICK_IN_OPP ||			worldModel->getPlayMode() == PM_FREE_KICK_OPP ||			worldModel->getPlayMode() == PM_OFFSIDE ||			worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||			worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||			worldModel->getPlayMode() == PM_CATCH_FAULT ||			worldModel->getPlayMode() == PM_BACK_PASS)		{			if (worldModel->getBall().getPos().getX() >= 0)			{//				senseX = 10;//				senseY = 22;			}		}        }			if (worldModel->getBall().getPos().getX() > 15 && 			(worldModel->getBody().getPostNum() == 6 ||			worldModel->getBody().getPostNum() == 7 /*||			worldModel->getBody().getPostNum() == 9 ||			worldModel->getBody().getPostNum() == 10 ||			worldModel->getBody().getPostNum() == 11*/))		if (worldModel->getPlayMode() == PM_KICK_IN_OPP ||			worldModel->getPlayMode() == PM_FREE_KICK_OPP ||			worldModel->getPlayMode() == PM_OFFSIDE ||			worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||			worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||			worldModel->getPlayMode() == PM_CATCH_FAULT ||			worldModel->getPlayMode() == PM_BACK_PASS)			defensePoint.x -= 8;		isPlayOn = true;			if (worldModel->getPlayMode() == PM_KICK_IN_OPP ||			worldModel->getPlayMode() == PM_FREE_KICK_OPP ||			worldModel->getPlayMode() == PM_OFFSIDE ||			worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||			worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||			worldModel->getPlayMode() == PM_CATCH_FAULT ||			worldModel->getPlayMode() == PM_BACK_PASS)			isPlayOn = false;    if(worldModel->getBody().getPostNum() >= 9)	    {		senseX = 13;		senseY = 13;    }			    if(worldModel->getBody().getPostNum() == 8)	    {		senseX = 8;		senseY = 4;		    }		        if(worldModel->getBody().getPostNum() < 6)	{		checkMarkPlayer(defensePoint,senseX, senseY);    		if(worldModel->getBall().getPos().getX() > -34)			defensePoint.x = library.positioning.getHomePoint(worldModel->getBody().					getUniNum()).x;	}		    if(worldModel->getBody().getPostNum() >= 9 && 		(worldModel->getPlayMode() == PM_KICK_IN_OPP ||		worldModel->getPlayMode() == PM_FREE_KICK_OPP ||		worldModel->getPlayMode() == PM_OFFSIDE ||		worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||		worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||		worldModel->getPlayMode() == PM_CATCH_FAULT ||		worldModel->getPlayMode() == PM_BACK_PASS))		checkMarkPlayer(defensePoint,senseX, senseY);    	    if(worldModel->getBody().getPostNum() == 7 && 		(worldModel->getBall().getPos().getX() < -30 || 		(worldModel->getPlayMode() == PM_KICK_IN_OPP ||		worldModel->getPlayMode() == PM_FREE_KICK_OPP ||		worldModel->getPlayMode() == PM_OFFSIDE ||		worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||		worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||		worldModel->getPlayMode() == PM_CATCH_FAULT ||		worldModel->getPlayMode() == PM_BACK_PASS)))		checkMarkPlayer(defensePoint,senseX, senseY);        if(worldModel->getBody().getPostNum() == 6 && 		(worldModel->getBall().getPos().getX() < -30 || 		(worldModel->getPlayMode() == PM_KICK_IN_OPP ||		worldModel->getPlayMode() == PM_FREE_KICK_OPP ||		worldModel->getPlayMode() == PM_OFFSIDE ||		worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||		worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||		worldModel->getPlayMode() == PM_CATCH_FAULT ||		worldModel->getPlayMode() == PM_BACK_PASS)))		checkMarkPlayer(defensePoint,senseX, senseY);        	if(worldModel->getBody().getPostNum() == 8 && 		worldModel->getBall().getPos().getX() < 10)	{		checkMarkPlayer(defensePoint,senseX, senseY);    		if(worldModel->getBall().getPos().getX() > -34)			defensePoint.x = library.positioning.getHomePoint(worldModel->getBody().					getUniNum()).x;	}	//	LOG << "defensePoint2 : " << defensePoint << endl;	defenseCommand = library.positioning.getPositioningCommand(defensePoint);//    checkForGoal(defensePoint, library);//	LOG << "defensePoint3 : " << defensePoint << endl;	return AD_NORMAL_VALUE;}void Defense::execute(Form &form, const Library &library){	LOG << "Defense::execute" << endl;	command = defenseCommand;	command->setCreator(AT_DEFENSE);}void Defense::checkGoalDist(Point &homePoint){    Point ballPoint,tirPoint;    if(worldModel->getBody().getPostNum() == 5)    {		tirPoint.x = -52.5;		tirPoint.y = 5;    }	    if(worldModel->getBody().getPostNum() == 2)    {		tirPoint.x = -52.5;		tirPoint.y = -5;    }    ballPoint.x = worldModel->getBall().getPos().getX();    ballPoint.y = worldModel->getBall().getPos().getY();    float ballToGoalDist, playerToGoalDist, homeToGoalDist;     ballToGoalDist = hypot(ballPoint.x - tirPoint.x, ballPoint.y - tirPoint.y);    playerToGoalDist = ballToGoalDist * .5;    homeToGoalDist = hypot(homePoint.x - tirPoint.x, homePoint.y - tirPoint.y);    if((ballToGoalDist < 20 || worldModel->getBall().getPos().getX() < -30) 			&& homeToGoalDist > playerToGoalDist)    {		Vector tirVector;		tirVector.setByPoints(tirPoint, homePoint);		tirVector.setAsPolar(playerToGoalDist, tirVector.getDirection());		homePoint.x = tirPoint.x + tirVector.getX();		homePoint.y = tirPoint.y + tirVector.getY();    }}int Defense::checkMarkPlayer(Point &homePoint, float senseX, float senseY){    Point markPoint[20];    int bestMarkPoint = 0;    float distsToMarkPoint[20];    int markPlayersNum = 0;    Point playerPoint;    	for (int j = 0; j < FULL_PLAYERS_NUM; j++) 		if (worldModel->getFullPlayer(TID_OPPONENT, j).isValid())		{					playerPoint.x = worldModel->getFullPlayer(TID_OPPONENT, j).getPos().getX();		    playerPoint.y = worldModel->getFullPlayer(TID_OPPONENT, j).getPos().getY();	    			float deltaY, deltaX;	    			deltaX = fabs(playerPoint.x - homePoint.x);		    deltaY = fabs(playerPoint.y - homePoint.y);		    			if(deltaX < senseX && deltaY < senseY)		    {				markPoint[markPlayersNum] = makeMarkPoint(playerPoint);								float playerDist = hypot(markPoint[markPlayersNum].x - worldModel->getBody().getPos().getX(), markPoint[markPlayersNum].y - worldModel->getBody().getPos().getY());				float homeDist = hypot(homePoint.x - markPoint[markPlayersNum].x, homePoint.y - markPoint[markPlayersNum].y);					distsToMarkPoint[markPlayersNum] = playerDist + homeDist;				markPlayersNum++;		    }		}    	for (int j = 0; j < HALF_PLAYERS_NUM; j++) 		if (worldModel->getHalfPlayer(TID_OPPONENT, j).isValid())		{					playerPoint.x = worldModel->getHalfPlayer(TID_OPPONENT, j).getPos().getX();		    playerPoint.y = worldModel->getHalfPlayer(TID_OPPONENT, j).getPos().getY();	    			float deltaY, deltaX;	    			deltaX = fabs(playerPoint.x - homePoint.x);		    deltaY = fabs(playerPoint.y - homePoint.y);		    			if(deltaX < senseX && deltaY < senseY)		    {				markPoint[markPlayersNum] = makeMarkPoint(playerPoint);								float playerDist = hypot(markPoint[markPlayersNum].x - worldModel->getBody().getPos().getX(), markPoint[markPlayersNum].y - worldModel->getBody().getPos().getY());				float homeDist = hypot(homePoint.x - markPoint[markPlayersNum].x, homePoint.y - markPoint[markPlayersNum].y);					distsToMarkPoint[markPlayersNum] = playerDist + homeDist;				markPlayersNum++;		    }		}        for(int j = 0; j < QUARTER_PLAYERS_NUM; j++)        if (worldModel->getQuarterPlayer(j).isValid())		{					playerPoint.x = worldModel->getQuarterPlayer(j).getPos().getX();		    playerPoint.y = worldModel->getQuarterPlayer(j).getPos().getY();	    			float deltaY, deltaX;	    			deltaX = fabs(playerPoint.x - homePoint.x);		    deltaY = fabs(playerPoint.y - homePoint.y);		    			if(deltaX < senseX && deltaY < senseY)		    {				markPoint[markPlayersNum] = makeMarkPoint(playerPoint);								float playerDist = hypot(markPoint[markPlayersNum].x - worldModel->getBody().getPos().getX(), markPoint[markPlayersNum].y - worldModel->getBody().getPos().getY());				float homeDist = hypot(homePoint.x - markPoint[markPlayersNum].x, homePoint.y - markPoint[markPlayersNum].y);					distsToMarkPoint[markPlayersNum] = playerDist + homeDist;				markPlayersNum++;		    }		}        if(markPlayersNum == 0)    {		LOG << "I did not mark any player" << endl;		return 0;    }	    for(int i = 0; i < markPlayersNum; i++)		if(distsToMarkPoint[bestMarkPoint] > distsToMarkPoint[i])	    	bestMarkPoint = i;    LOG << "I am marking a point that is : " << markPoint[bestMarkPoint] << endl;	homePoint = markPoint[bestMarkPoint]; 	return 0;}Point Defense::makeMarkPoint(Point playerPoint){    Point markPoint, markPoint1, markPoint2;	float plusX;    	plusX = 2;    plusX += ((hypot(playerPoint.x - worldModel->getBody().getPos().getX(), 			playerPoint.y - worldModel->getBody().getPos().getY())) / 10);    	if(plusX > 3)		plusX = 3;		//making markPoint1    	markPoint1.x = playerPoint.x - plusX;    markPoint1.y = playerPoint.y;//making markPoint2	Vector markVector;	markVector.setByPoints(playerPoint, worldModel->getBall().getPos());	markVector.setAsPolar(3, markVector.getDirection());	markPoint2.x = playerPoint.x + markVector.getX();	markPoint2.y = playerPoint.y + markVector.getY();    if (worldModel->getBall().getPos().getX() < 			playerPoint.x && markPoint.x < -43)    {//Algorithm1 remarked.	/*		float markDist1, markDist2;		markDist1 = hypot(BasicPos[worldModel->getBody().getUniNum() - 1].x - markPoint1.x, BasicPos[worldModel->getBody().getUniNum() - 1].y - markPoint1.y);		markDist2 = hypot(BasicPos[worldModel->getBody().getUniNum() - 1].x - markPoint2.x, BasicPos[worldModel->getBody().getUniNum() - 1].y - markPoint2.y);		if(markDist1 < markDist2)		    markPoint = markPoint1;		else	    	markPoint = markPoint2;   *///Algorithm2 is used		markPoint.x = (markPoint2.x - markPoint1.x) * .8 + markPoint1.x;		markPoint.y = (markPoint2.y - markPoint1.y) * .8 + markPoint1.y;	}	else		markPoint = markPoint1;    	if (!isPlayOn && worldModel->getBall().getPos().getX() > -10 &&		worldModel->getBody().getPostNum() >= 6 && 		worldModel->getBody().getPostNum() != 8)	{		markPoint = markPoint2;	}				return markPoint;}void Defense::checkForGoal(Point &homePoint, const Library &library){    Point tirPoint;    float currentDir;	    if(worldModel->getBody().getPostNum() == 5)    {		if(fabs(library.positioning.getHomePoint(worldModel->getBody().getUniNum()).x + 52.5) < 11 && 				fabs(library.positioning.getHomePoint(worldModel->getBody().getUniNum()).y - 7) < 7)		{	    	tirPoint.x = -51;	    				if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 					tirPoint.x > worldModel->getBody().getPos().getX())				tirPoint.x = worldModel->getBody().getPos().getX();	    				float A = hypot(worldModel->getBall().getPos().getX() + 52.5, 						worldModel->getBall().getPos().getY() - 7) / 						hypot(worldModel->getBall().getPos().getX() + 52.5, 						worldModel->getBall().getPos().getY() - 3);							   		A = A * A * A;	   					float B = 4;	    	float M = (A * B) / (A + 1);//	    	float N = B - M;	    				tirPoint.y = 7 - M;	    				if(worldModel->getBall().getPos().getY() >= tirPoint.y)				currentDir = 90;	    	else				currentDir = -90;			if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 						tirPoint.x > worldModel->getBody().getPos().getX())				tirPoint.x = worldModel->getBody().getPos().getX();			homePoint = tirPoint;			gotoTir(tirPoint, currentDir);		}    }    	if(worldModel->getBody().getPostNum() == 2)    {		if(fabs(library.positioning.getHomePoint(worldModel->getBody().getUniNum()).x + 52.5) < 13 && 			fabs(library.positioning.getHomePoint(worldModel->getBody().getUniNum()).y + 7) < 7)		{		    tirPoint.x = -51;					    if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 					tirPoint.x > worldModel->getBody().getPos().getX())				tirPoint.x = worldModel->getBody().getPos().getX();		    			float A = hypot(worldModel->getBall().getPos().getX() + 52.5, 						worldModel->getBall().getPos().getY() + 7) / 						hypot(worldModel->getBall().getPos().getX() + 52.5, 						worldModel->getBall().getPos().getY() + 3);		    			A = A * A * A;		    			float B = 4;		    float M = (A * B) / (A + 1);//		    float N = B - M;		    			tirPoint.y = -7 + M;		    			if(worldModel->getBall().getPos().getY() >= tirPoint.y)				currentDir = 90;	    	else				currentDir = -90;	    		    	if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 					tirPoint.x > worldModel->getBody().getPos().getX())				tirPoint.x = worldModel->getBody().getPos().getX();					homePoint = tirPoint;			gotoTir(tirPoint, currentDir);		}	    }}void Defense::gotoTir(Point tirPoint, float currentAngle){    Vector lenVector;        lenVector.setByPoints(worldModel->getBody().getPos(), tirPoint);        if (lenVector.getMagnitude() <= .2)    {        float deltaAngle = currentAngle - worldModel->getBody().getBodyDir();        deltaAngle = Degree::normalizeAngle(deltaAngle);		defenseCommand = TurnAngle(AT_DEFENSE, deltaAngle,worldModel->getBody()).getCommand();    }    else    {		if(lenVector.getMagnitude() >= 6)    		defenseCommand = DashNormalGotoPoint(AT_DEFENSE, tirPoint, 1.5, worldModel->getBody()).getCommand();		else    		defenseCommand = PreciseGotoPoint(AT_DEFENSE, tirPoint, .2, worldModel->getBody(), 1).getCommand();    }}Point Defense::getHomePoint(int playerUniNum){	return defensePoint;}