goalieintercept.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

/*
 *  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 GoalieCatch
GoalieCatch::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 GoalieIntercept
GoalieIntercept::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