/* *  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 <Degree.h>#include <Logger.h>#include <Defines.h>#include <SRPDribble.h>#include <InterceptCalculate.h>//#define SRPDRIBBLE_LOGLEVEL1using namespace std;SRPDribble::SRPDribble(const WorldModel *worldModel): AdvancedAction(worldModel){}SRPDribble::~SRPDribble(){}void SRPDribble::execute(Form &form, const Library &library){	LOG << "SRPDribble::execute" << endl;	command = srpCommand;	command->setCreator(AT_SRPDRIBBLE);}void SRPDribble::getDangerActionValue(){//KOMAKE KHODAM : INHA CHIST?!?    CKickGoToBallPlayer *GetBodyGoToBall, *GetBestOppGoToBall;    int bodyListNumber, bestOppListNumber;        for (unsigned i = 0; i < worldModel->getCurKickCalculate().getSimulatesNum(); i++)	{	    if (!worldModel->getCurKickCalculate().getSimulate(i).isValid())	    {			deltaToFirstTime[i] = -999;			continue;	    }	    if (!worldModel->getCurKickCalculate().getSimulate(i).existFastestPlayer())	    {			deltaToFirstTime[i] = -999;	    	continue;	    }		    bodyListNumber = worldModel->getCurKickCalculate().getSimulate(i).getCheckedListNum(worldModel->getBody());	    if (bodyListNumber == NOVALUE)	    {			deltaToFirstTime[i] = -999;			sarePaTime[i] = -999;			continue;	    }		    	    if (!worldModel->getCurKickCalculate().getSimulate(i).getOppCheckedPlayersNum())	    {			deltaToFirstTime[i] = DEFAULT_SHARE_CYCLES;			sarePaTime[i] = worldModel->getCurKickCalculate().getSimulate(i).						getCheckedInterPlayer(bodyListNumber).getInterTime();			oppGtbTime[i] = 999;	    	continue;	    }		    				    bestOppListNumber = MAX_CHECK_PLAYERS + 1;	    	    for (unsigned o = 0; o < worldModel->getCurKickCalculate().getSimulate(i).getCheckedPlayersNum(); o++)	    {			if(worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(o).getPlayer().getTeamId() == TID_TEAMMATE) 		 	   continue;			if (bestOppListNumber == MAX_CHECK_PLAYERS + 1 ||		    	worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(o).getInterTime() < 		    	worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bestOppListNumber).getInterTime())		    	bestOppListNumber = o;	         	    	    }	    		if (bestOppListNumber == MAX_CHECK_PLAYERS + 1)		{			deltaToFirstTime[i] = DEFAULT_SHARE_CYCLES;		}		else			{			deltaToFirstTime[i] = (int)(worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bestOppListNumber).getInterTime() -	                    	     worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bodyListNumber).getInterTime());		}				if (deltaToFirstTime[i] < 0)			deltaToFirstTime[i] *= 15;		else if (deltaToFirstTime == 0)			deltaToFirstTime[i] -= 4;				sarePaTime[i] = worldModel->getCurKickCalculate().getSimulate(i).			getCheckedInterPlayer(bodyListNumber).getInterTime();		oppGtbTime[i] = worldModel->getCurKickCalculate().getSimulate(i).				getCheckedInterPlayer(bestOppListNumber).getInterTime();	}	}void SRPDribble::getDangerFieldValue(){    int bodyListNumber;    Point goToBallPoint;    float dangerX = 50, dangerY = 31.5;        for (unsigned i = 0; i < worldModel->getCurKickCalculate().getSimulatesNum(); i++)	{	    bodyListNumber = worldModel->getCurKickCalculate().getSimulate(i).getCheckedListNum(worldModel->getBody());	    		if (bodyListNumber == NOVALUE)		{			dangerFieldValue[i] = -1000;			continue;		}						goToBallPoint = worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bodyListNumber).getInterPoint();	    dangerFieldValue[i] = 0;	    	    	    if(goToBallPoint.x > dangerX)			dangerFieldValue[i] -= (goToBallPoint.x - dangerX) / 1;	    if(fabs(goToBallPoint.y) > dangerY)			dangerFieldValue[i] -= (fabs(goToBallPoint.y) - dangerY) / 1;	}}void SRPDribble::getSpeedValue(){    int bodyListNumber;    Point goToBallPoint;    float normalDistance = 8;    float sarePaPointDistance;    float normalWeight = 4;                       	for (unsigned i = 0; i < worldModel->getCurKickCalculate().getSimulatesNum(); i++)	{	    bodyListNumber = worldModel->getCurKickCalculate().getSimulate(i).getCheckedListNum(worldModel->getBody());		if (bodyListNumber == NOVALUE)		{			speedValue[i] = -1000;			continue;		}	    goToBallPoint = worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bodyListNumber).getInterPoint();	    sarePaPointDistance = hypot(goToBallPoint.x - worldModel->getBall().getPos().getX(), goToBallPoint.y - worldModel->getBall().getPos().getY());	    	    if(sarePaPointDistance <= normalDistance)			speedValue[i] = normalWeight + (sarePaPointDistance - normalDistance);	    if(sarePaPointDistance > normalDistance)			speedValue[i] = normalWeight - (sarePaPointDistance - normalDistance) * 6;	    if (i == 40)	    {		LOG << "salam salam sad ta salam : distToSrpPoint : " << sarePaPointDistance << endl;		LOG << "speedValue[40] : " << speedValue[i] << endl;	    }//pak nashavad	    if(speedValue[i] < 0)//			speedValue[i] = 0;	    		Vector gtbVector;	    float enemySpeedAngle;	    		gtbVector.setByPoints(worldModel->getBody().getPos(), goToBallPoint);	    enemySpeedAngle = gtbVector.getDirection() - worldModel->getBody().getBodyDir();	    enemySpeedAngle = Degree::normalizeAngle(enemySpeedAngle);	    enemySpeedAngle = fabs(enemySpeedAngle);	    if(enemySpeedAngle < 17)		speedValue[i] += 3.5;	    if (enemySpeedAngle > 32)			speedValue[i] -= 5;	}    }void SRPDribble::getNextStatusValue(){    Point targetPoint;    targetPoint.x = 51;    if(worldModel->getBody().getPos().getX() < 36)		targetPoint.y = worldModel->getBody().getPos().getY();    else		targetPoint.y = 0;	    int bodyListNumber;//    Point nextBodyPoint;	    Vector dribbleTargetVector, goToBallTargetVector;    float deltaAngleToTarget;		for (unsigned i = 0; i < worldModel->getCurKickCalculate().getSimulatesNum(); i++)	{	    if (!worldModel->getCurKickCalculate().getSimulate(i).isValid())	    {			nextStatusValue[i] = -999;			continue;	    }	    if (!worldModel->getCurKickCalculate().getSimulate(i).existFastestPlayer())	    {			nextStatusValue[i] = -999;	    	continue;	    }		    bodyListNumber = worldModel->getCurKickCalculate().getSimulate(i).getCheckedListNum(worldModel->getBody());		if (bodyListNumber == NOVALUE)		{			nextStatusValue[i] = -999;			continue;		}	    	    myGoToBallPoint[i] = worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bodyListNumber).getInterPoint();	    	    dribbleTargetVector.setByPoints(worldModel->getBody().getPos(), targetPoint);	    goToBallTargetVector.setByPoints(worldModel->getBody().getPos(), myGoToBallPoint[i]);	     		    deltaAngleToTarget = goToBallTargetVector.getDirection() - dribbleTargetVector.getDirection();	    deltaAngleToTarget = Degree::normalizeAngle(deltaAngleToTarget);	    deltaAngleToTarget = fabs(deltaAngleToTarget);	    	    nextStatusValue[i] = myGoToBallPoint[i].x - worldModel->getBody().getPos().getX();	}}void SRPDribble::getPathValue(){	float firstXPathStep = 42;	Point targetPoint;	targetPoint.x = 51;	if(worldModel->getBody().getPos().getX() < firstXPathStep)	    targetPoint.y = worldModel->getBody().getPos().getY();	else	    targetPoint.y = 0;    	    int bodyListNumber;//    Point nextBodyPoint;	    Vector dribbleTargetVector, goToBallTargetVector;    float deltaAngleToTarget;		for (unsigned i = 0; i < worldModel->getCurKickCalculate().getSimulatesNum(); i++)	{	    if (!worldModel->getCurKickCalculate().getSimulate(i).isValid())	    {			pathValue[i] = -999;			continue;	    }	    if (!worldModel->getCurKickCalculate().getSimulate(i).existFastestPlayer())	    {			pathValue[i] = -999;	    	continue;	    }		    bodyListNumber = worldModel->getCurKickCalculate().getSimulate(i).getCheckedListNum(worldModel->getBody());			if (bodyListNumber == NOVALUE)		{			pathValue[i] = -999;			continue;		}	    	    myGoToBallPoint[i] = worldModel->getCurKickCalculate().getSimulate(i).getCheckedInterPlayer(bodyListNumber).getInterPoint();	    	    dribbleTargetVector.setByPoints(Point(worldModel->getBody().getPos().getX(), worldModel->getBody().getPos().getY()), targetPoint);	    goToBallTargetVector.setByPoints(Point(worldModel->getBody().getPos().getX(), worldModel->getBody().getPos().getY()), myGoToBallPoint[i]);	     		    deltaAngleToTarget = goToBallTargetVector.getDirection() - dribbleTargetVector.getDirection();	    deltaAngleToTarget = Degree::normalizeAngle(deltaAngleToTarget);	    deltaAngleToTarget = fabs(deltaAngleToTarget);	    	    pathValue[i] = (180 - deltaAngleToTarget) / 15.00;  			if (targetPoint.y != 0)		{			deltaAngleToTarget = fabs(goToBallTargetVector.getDirection());	    	pathValue[i] = fmax((180 - deltaAngleToTarget) / 15.00, pathValue[i]);  		}	}    }void SRPDribble::getOppBallVectorValue(){	float oppDeltaDir;	Point oppFastestPoint;			Vector meToInterPointVector, oppToInterPointVector;	for (unsigned i = 0; i < worldModel->getCurKickCalculate().getSimulatesNum(); i++)	{	    if (!worldModel->getCurKickCalculate().getSimulate(i).isValid())	    {			oppBallVectorValue[i] = -999;			continue;	    }	    if (!worldModel->getCurKickCalculate().getSimulate(i).existFastestPlayer())	    {			oppBallVectorValue[i] = -999;	    	continue;	    }