/* *  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 <BasicTurn.h>#include <BasicDash.h>#include <Basics.h>#include <GoaliePositioning.h>using namespace std;GoaliePositioning::GoaliePositioning(const WorldModel *worldModel):		AdvancedAction(worldModel){}void GoaliePositioning::setGoalieParams(){    Point ballPoint;    float changeXDist = 1;    float changeYDistBadAngle = 1;    float minGoalieX = -48.5;    float virtualMinGoalieX;//  	float overLoadDistByNoTurn;// 		float overLoadDistByTurn;    float virtualGoalieX, virtualGoalieYIn52, 		virtualGoalieYIn44, virtualGoalieYInNow;    	Line midAngleLine, upWallLine, downWallLine, directWallLine;     //KOMAK		/*KOMAK*/ float goalieXHome = -47;	//KOMAK	if (worldModel->getPlayMode() == PM_PENALTY_SETUP ||		worldModel->getPlayMode() == PM_PENALTY_SETUP_OPP ||		worldModel->getPlayMode() == PM_PENALTY_READY_OPP ||		worldModel->getPlayMode() == PM_PENALTY_READY)	{		goalieXHome = -40;		changeXDist = 2;	}		else	{//	 LATEST TRICK		if (config["Agent"]["Public"]["LatestTrick"].asBool() && 		worldModel->getBall().getPos().getX() > -3)	{		if (worldModel->getBall().getPos().getX() > -34 &&			worldModel->getBall().getPos().getX() < -15)			goalieXHome = Basics::reRate(worldModel->getBall().getPos().getX(),					-25, -15, -47, -38);				if (worldModel->getBall().getPos().getX() > -15 &&			worldModel->getBall().getPos().getX() < 0)			goalieXHome = Basics::reRate(worldModel->getBall().getPos().getX(),					-15, 0, -38, -20);			if (worldModel->getBall().getPos().getX() >= 0)			goalieXHome = Basics::reRate(worldModel->getBall().getPos().getX(),					0, 54, -24, -15);		if (goalieXHome > -32)			goalieXHome = -32;	}//	LATEST TRICK 					}	    virtualGoalieX = goalieXHome;    virtualGoalieYIn52 = 6;    virtualGoalieYIn44 = 6;	ballPoint.x = worldModel->getBall().getPos().getX() + 					worldModel->getBall().getVel().getX();						ballPoint.y = worldModel->getBall().getPos().getY() + 					worldModel->getBall().getVel().getY();    if(ballPoint.x < -52.4)		ballPoint.x = -52.4;    virtualMinGoalieX = -52.5 + float((ballPoint.x - (-52.5)) / 2.8);           if(worldModel->getBall().getPos().getX() < -25 && 		worldModel->getBody().getPos().getX() < virtualGoalieX)    {		if (worldModel->getBody().getPos().getX() > virtualMinGoalieX)	   		virtualGoalieX = worldModel->getBody().getPos().getX(); 		else if (virtualMinGoalieX < virtualGoalieX) 			virtualGoalieX = virtualMinGoalieX;    }    if(virtualGoalieX < minGoalieX)		virtualGoalieX = minGoalieX;    if(fabs(virtualGoalieX - worldModel->getBody().getPos().getX()) 		< changeXDist)		virtualGoalieX = worldModel->getBody().getPos().getX();        virtualGoalieYInNow = virtualGoalieYIn44 + ((-44 - 							virtualGoalieX) * (virtualGoalieYIn52 - 							virtualGoalieYIn44)) / (-44 - (-52.5));	     Point tirUpPoint, tirDownPoint;    Vector tirUpVector, tirDownVector, midAngleVector;    float deltaAngle, betweenAngle;        tirUpPoint.x = tirDownPoint.x = -52.5;    tirUpPoint.y = 7;    tirDownPoint.y = -7;        tirUpVector.setByPoints(ballPoint, tirUpPoint);    tirDownVector.setByPoints(ballPoint, tirDownPoint);        deltaAngle = tirUpVector.getDirection() - tirDownVector.getDirection();    deltaAngle = Degree::normalizeAngle(deltaAngle);    deltaAngle = fabs(deltaAngle);    deltaAngle /= 2.00;        betweenAngle = tirUpVector.getDirection() + deltaAngle;    betweenAngle = Degree::normalizeAngle(betweenAngle);        midAngleVector.setAsPolar(1, betweenAngle);    midAngleLine.setBySourceDir(ballPoint, midAngleVector.getDirection());        upWallLine.setByPoints(Point(virtualGoalieX, virtualGoalieYInNow), 							Point(-52.5, virtualGoalieYIn52));						    downWallLine.setByPoints(Point(virtualGoalieX, -virtualGoalieYInNow), 							Point(-52.5, -virtualGoalieYIn52));							    directWallLine.setByPoints(Point(virtualGoalieX, -1), 								Point(virtualGoalieX, 1));	LOG << "directWallLine : a, b, c = " << directWallLine.getA() << "," << directWallLine.getB() << "," << directWallLine.getC() << endl;	LOG << "midAngleLine : a, b, c = " << midAngleLine.getA() << "," << midAngleLine.getB() << "," << midAngleLine.getC() << endl;	LOG << "midAngleLine : direction = " << midAngleLine.getDirection() << endl;	LOG << "virtualGoalieX = " << virtualGoalieX << endl;    midAngleLine.getLineIntersect(directWallLine, goalieHomePoint);    LOG << "goalieHomePoint 1 : " << goalieHomePoint << endl;        if(goalieHomePoint.y > virtualGoalieYInNow)    {		midAngleLine.getLineIntersect(upWallLine, goalieHomePoint);    	LOG << "goalieHomePoint 2 : " << goalieHomePoint << endl;    }    else if(goalieHomePoint.y < (-virtualGoalieYInNow))	{	    midAngleLine.getLineIntersect(downWallLine, goalieHomePoint);    	LOG << "goalieHomePoint 3 : " << goalieHomePoint << endl;	}    mainIdealBetweenAngle = midAngleVector.getDirection();         Vector goalieHomeVector;    goalieHomeVector.setByPoints(worldModel->getBody().getPos(), 								goalieHomePoint);        	float localDeltaAngle = goalieHomeVector.getDirection() - 							worldModel->getBody().getBodyDir();							    localDeltaAngle = Degree::normalizeAngle(localDeltaAngle);    localDeltaAngle = fabs(localDeltaAngle);    	if((localDeltaAngle >= 15 || localDeltaAngle >= 165) && 		fabs(goalieHomePoint.y - worldModel->getBody().getPos().getY()) <= 		changeYDistBadAngle)		goalieHomePoint.y = worldModel->getBody().getPos().getY();    LOG << "goalieHomePoint 4 : " << goalieHomePoint << endl;    float idealBodyAngle = mainIdealBetweenAngle - 90;    idealBodyAngle = Degree::normalizeAngle(idealBodyAngle);    Line idealAngleLine, ballToHomeLine;    Point bodyPoint;	    bodyPoint.x = worldModel->getBody().getPos().getX();    bodyPoint.y = worldModel->getBody().getPos().getY();    	idealAngleLine.setBySourceDir(bodyPoint, idealBodyAngle);    Line tirUpLine, tirDownLine;    tirUpLine.setByPoints(ballPoint, tirUpPoint);    tirDownLine.setByPoints(ballPoint, tirDownPoint);    LOG << "goalieHomePoint 5 : " << goalieHomePoint << endl;    float miniChangeDist = .3;	    if(worldModel->getBall().getBodyVec().getMagnitude() >= 10)		miniChangeDist += (worldModel->getBall().getBodyVec().getMagnitude() 							- 10) * .05;    	if(worldModel->getBall().getBodyVec().getMagnitude() >= 25)		miniChangeDist = 1.2;    	if (worldModel->getPlayMode() != PM_PLAY_ON)    	miniChangeDist = 5;        	if(mainIdealBetweenAngle > -122 && mainIdealBetweenAngle < 0)    {		goalieHomePoint = shootToTirSimulate(tirUpPoint, goalieHomePoint, 							idealBodyAngle, .00/*firstDashLength*/, 							6/*maxChangeDistance*/,miniChangeDist);							    	LOG << "goalieHomePoint 6 : " << goalieHomePoint << endl;    }	    if(mainIdealBetweenAngle < 122 && mainIdealBetweenAngle > 0)    {		goalieHomePoint = shootToTirSimulate(tirDownPoint, goalieHomePoint, 							idealBodyAngle, .00/*firstDashLength*/, 							6/*maxChangeDistance*/,miniChangeDist);							    	LOG << "goalieHomePoint 7 : " << goalieHomePoint << endl;    }        if(hypot(goalieHomePoint.x - worldModel->getBody().getPos().getX(), 		goalieHomePoint.y - worldModel->getBody().getPos().getY()) < 3)    {    	if(mainIdealBetweenAngle < -122 && mainIdealBetweenAngle > -160)     	{			Point tempSource;			idealAngleLine.getLineIntersect(tirUpLine, tempSource);			goalieHomePoint = shootToTirSimulate(tempSource, goalieHomePoint, 								idealBodyAngle, .00/*firstDashLength*/, 								7/*maxChangeDistance*/, .3);								 		   	LOG << "goalieHomePoint 8 : " << goalieHomePoint << endl;    	}			    if(mainIdealBetweenAngle > 124 && mainIdealBetweenAngle < 160) 	    {			Point tempSource;			idealAngleLine.getLineIntersect(tirDownLine, tempSource);			goalieHomePoint = shootToTirSimulate(tempSource, goalieHomePoint, 								idealBodyAngle, .00/*firstDashLength*/, 								7/*maxChangeDistance*/, .3);								  		  	LOG << "goalieHomePoint 9 : " << goalieHomePoint << endl;    	}    }    LOG << "goalieHomePoint 10 : " << goalieHomePoint << endl;        ballToHomeLine.setByPoints(goalieHomePoint, ballPoint);    float permitedDist = 1.2;    	if(hypot(goalieHomePoint.x - worldModel->getBody().getPos().getX(), 		goalieHomePoint.y - worldModel->getBody().getPos().getY()) < permitedDist)		idealAngleLine.getLineIntersect(ballToHomeLine, goalieHomePoint);   	LOG << "goalieHomePoint 11 : " << goalieHomePoint << endl;    if(goalieHomePoint.x < -50.5 )		goalieHomePoint.x = -50.5;   	LOG << "goalieHomePoint 12 : " << goalieHomePoint << endl;}void GoaliePositioning::setGoalieParamsLineMode(){    float goalieXHomeLineMode = -47;    float goalieXHomeEmg = -49.5;    float mainGoalieXHomeLM;	float minGoalieXLM = -50;	float virtualMinGoalieXLM;	if (worldModel->getPlayMode() == PM_PENALTY_SETUP ||		worldModel->getPlayMode() == PM_PENALTY_SETUP_OPP ||		worldModel->getPlayMode() == PM_PENALTY_READY_OPP ||		worldModel->getPlayMode() == PM_PENALTY_READY)	{	}	else	{///////////	LATEST TRICK	if (config["Agent"]["Public"]["LatestTrick"].asBool() &&		worldModel->getBall().getPos().getX() > -3)	{		if (worldModel->getBall().getPos().getX() > -34 &&			worldModel->getBall().getPos().getX() < -15)			goalieXHomeLineMode = Basics::reRate(worldModel->getBall().getPos().getX(),					-25, -15, -47, -38);				if (worldModel->getBall().getPos().getX() > -15 &&			worldModel->getBall().getPos().getX() < 0)			goalieXHomeLineMode = Basics::reRate(worldModel->getBall().getPos().getX(),					-15, 0, -38, -20);			if (worldModel->getBall().getPos().getX() >= 0)			goalieXHomeLineMode = Basics::reRate(worldModel->getBall().getPos().getX(),					0, 54, -24, -15);		if (goalieXHomeLineMode > -32)			goalieXHomeLineMode = -32;	}//	LATEST TRICK //////////		}	    Vector upTirToBall, downTirToBall, goalToBall;    Point ballPoint;        ballPoint.x = worldModel->getBall().getPos().getX() + 					worldModel->getBall().getVel().getX();					    ballPoint.y = worldModel->getBall().getPos().getY() + 					worldModel->getBall().getVel().getY();    mainGoalieXHomeLM = goalieXHomeLineMode;	virtualMinGoalieXLM = -52.5 + float((ballPoint.x - (-52.5)) / 2.8);   	minGoalieXLM = max(minGoalieXLM, virtualMinGoalieXLM);	if (worldModel->getBody().getPos().getX() < mainGoalieXHomeLM)	{		if (worldModel->getBody().getPos().getX() > minGoalieXLM)			mainGoalieXHomeLM = worldModel->getBody().getPos().getX();		}	    upTirToBall.setByPoints(Point(-52.5, 7), ballPoint);    downTirToBall.setByPoints(Point(-52.5, -7), ballPoint);    goalToBall.setByPoints(Point(-52.5, 0), ballPoint);	    float deltaAngleToUpTir = upTirToBall.getDirection() - 90;    float deltaAngleToDownTir = downTirToBall.getDirection() - (-90);    float deltaAngleToGoal;    	if (ballPoint.y > 0)    	deltaAngleToGoal = goalToBall.getDirection() - 90;	else    	deltaAngleToGoal = goalToBall.getDirection() - (-90);			    deltaAngleToUpTir = Degree::normalizeAngle(deltaAngleToUpTir);    deltaAngleToDownTir = Degree::normalizeAngle(deltaAngleToDownTir);    deltaAngleToGoal = Degree::normalizeAngle(deltaAngleToGoal);	    deltaAngleToUpTir = fabs(deltaAngleToUpTir);    deltaAngleToDownTir = fabs(deltaAngleToDownTir);    deltaAngleToGoal = fabs(deltaAngleToGoal);	LOG << "Goalie LOG 1 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 		<< " " << goalieXHomeEmg << endl;    if(worldModel->getBall().getPos().getY() > 0 && 		deltaAngleToUpTir < 45 && upTirToBall.getMagnitude() < 15)		{			mainGoalieXHomeLM = goalieXHomeEmg;			LOG << "GOALIE Change Function 1" << endl;			setGoalieParams();			return;		}		LOG << "Goalie LOG 2 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 		<< " " << goalieXHomeEmg << endl;		    if(worldModel->getBall().getPos().getY() < 0 && 		deltaAngleToDownTir < 45 && downTirToBall.getMagnitude() < 15)		{			mainGoalieXHomeLM = goalieXHomeEmg;			LOG << "GOALIE Change Function 2" << endl;			setGoalieParams();			return;		}	LOG << "Goalie LOG 3 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 		<< " " << goalieXHomeEmg << endl;    LOG << "deltaAngleToGoal : " << deltaAngleToGoal << endl;    LOG << "goalToBall.Length : " << goalToBall.getMagnitude() << endl;