intercept.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 <Line.h>
#include <Degree.h>
#include <Vector.h>
#include <Logger.h>
#include <Command.h>
#include <Intercept.h>
#include <BasicTurn.h>

#include <AdvancedAgent.h>


using namespace std;
using namespace Degree;

// class DribbleIntercept
DribbleIntercept::DribbleIntercept(const WorldModel *worldModel):
		AdvancedAction(worldModel)
{
}

void DribbleIntercept::execute(Form &form, const Library &library)
{
	LOG << "DribbleIntercept::execute" << endl;

	command = dashCommand;
}

float DribbleIntercept::getValue(const Library &library)
{
	LOG << "DribbleIntercept::getValue" << endl;

	Body simBody, nextBody;
	Ball simBall, nextBall;
	
	simBody = worldModel->getBody();
	simBall = worldModel->getBall();

	nextBody = worldModel->getBody();
	nextBall = worldModel->getBall();

	float kickableArea, virtualKickableArea;
	float dashPower;
	int dribbleGTBTime = 0;
	
	kickableArea = worldModel->getBody().getSize() + 
			worldModel->getBody().getKickableMargin() + worldModel->getBall().getSize();
			
	virtualKickableArea = kickableArea + .2; // rate 1.06 for caution 
	
/*	float deltaAngleToBallVel;
	deltaAngleToBallVel = worldModel->getBody()->getBodyDir - 
					worldModel->getBall().getVel().getDirection();
	
	deltaAngleToBallVel = Degree::normalizeAngle(deltaAngleToBallVel);*/
	
	
	dashPower = 100;
	if (worldModel->getBody().getStamina() < max(worldModel->getBody().getRecoverDecThr(),
								 worldModel->getBody().getEffortDecThr()) *
							 worldModel->getBody().getStaminaMax() + 150) // 150 for caution
		dashPower = worldModel->getBody().getStaminaIncMax();
	
	//dashCommand.dash(dashPower);	 
	dashCommand = new DashCommand(AT_INTERCEPT, dashPower);

	nextBody.simulateByAction(dashCommand);
	nextBody.simulateByDynamics();
	nextBall.simulateByDynamics(nextBody);	
	
	while (!(simBall.getBodyVec().getMagnitude() < kickableArea ||
		   (simBall.getBodyVec().getMagnitude() < virtualKickableArea
		   && nextBall.getBodyVec().getMagnitude() < virtualKickableArea) || 
		   dribbleGTBTime > 30))
	{
		simBody.simulateByAction(dashCommand);
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);	

		nextBody.simulateByAction(dashCommand);
		nextBody.simulateByDynamics();
		nextBall.simulateByDynamics(nextBody);	
		
		dribbleGTBTime++;
	}
	
	if (simBall.getBodyVec().getMagnitude() < kickableArea ||
		   (simBall.getBodyVec().getMagnitude() < virtualKickableArea
		   && nextBall.getBodyVec().getMagnitude() < virtualKickableArea))
		return AD_ALWAYS_RUN_VALUE;
	else
	{
		delete dashCommand;
		return AD_MIN_VALUE;
	}
	
}


// class Intercept
Intercept::Intercept(const WorldModel *worldModel): AdvancedAction(worldModel)
{
}

float Intercept::getValue(const Library &library)
{
	LOG << "Intercept::getValue" << endl;

	Body simBody;
	Ball simBall;
	Body waitBody;
	Ball waitBall;
	simBody = worldModel->getBody();
	simBall = worldModel->getBall();
	DashCommand tempDashCommand(AT_INTERCEPT, 0);
	float bodyKickableArea;
	float extraKickableForNoise;
	float extraKickableForAngleNoise = 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);
	}
	
	Point defaultTurnPoint;
	if (worldModel->getBody().getPos().getX() > 32)
		defaultTurnPoint = Point(52, 0);
	else
		defaultTurnPoint = Point(worldModel->getBody().getPos().getX() + 10,
				worldModel->getBody().getPos().getY());

	bodyKickableArea = worldModel->getBody().getSize() + worldModel->getBody().getKickableMargin() +
					   worldModel->getBall().getSize();
	gtbPointsNumber = 0;
	for (int i = 0; i < MAX_GTB_POINTS_NUM; i++)
	{
		gtbTime[i] = 999; 
		minBallDistance[i] = bodyKickableArea;
		extraGTBTime[i] = 0;
		gtbPointWeight[i] = 0;
		gtbCommand[i] = TurnToPoint(AT_INTERCEPT, defaultTurnPoint, worldModel->getBody()).getCommand();
	}
	
////////NoAction
	simBody = worldModel->getBody();
	simBall = worldModel->getBall();
	waitBody = simBody;
	waitBall = simBall;
	
	

	for (int i = 0; i <= WAIT_FOR_BALL_CYCLES; i++)
	{
//		float ExtraKickableForNoise = .15 * sqrt(MainGoToBallTime);
		extraKickableForNoise = -.15;
		if (waitBall.getBodyVec().getMagnitude() <= (bodyKickableArea + extraKickableForNoise))
		{
			gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 
			extraGTBTime[gtbPointsNumber] = i;
			mainGtbTime[gtbPointsNumber] = mainGoToBallTime + 
								extraGTBTime[gtbPointsNumber];
			gtbStatus[gtbPointsNumber] = 0;					
			gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);
//*tof*/			gtbTime[gtbPointsNumber] -= 2;				
			minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();
/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyKickableArea - minBallDistance[gtbPointsNumber]) , (float).3) * 10; 		
			distToBegin[gtbPointsNumber] = hypot(waitBall.getPos().getX() - worldModel->getBall().getPos().getX(), waitBall.getPos().getY() - worldModel->getBall().getPos().getY());
			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 < 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_INTERCEPT, dashPower);
		simBody.simulateByAction(dynamic_cast<Command *> (&tempDashCommand));
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);
//KOMAK		simBody.setStamina(simBody.getStamina() - DashPower);

		
		for (int i = 0; i <= WAIT_FOR_BALL_CYCLES; i++)
		{
			extraKickableForNoise = worldModel->getBall().getBodyVec().getMagnitude() * .07;
/////////////////////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(normalizeAngle(interceptGTBVector.getDirection() - worldModel->getBody().getBodyDir())) < 2)
			{
				float deltaAngleToBallAngle;
				deltaAngleToBallAngle = simBody.getBodyDir() - simBall.getVel().getDirection();
				deltaAngleToBallAngle = normalizeAngle(deltaAngleToBallAngle);
				if (interceptGTBVector.getMagnitude() < 8)
				{
					if (fabs(deltaAngleToBallAngle) < 90)
						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;
					else if (fabs(deltaAngleToBallAngle) < 120)
						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .2;
					else if (fabs(deltaAngleToBallAngle) >= 120)
						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * .4;
				}
				else
				{
					if (fabs(deltaAngleToBallAngle) < 90)
						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;
					else if (fabs(deltaAngleToBallAngle) < 120)
						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;
					else if (fabs(deltaAngleToBallAngle) >= 120)
						extraKickableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.5;
				}
			}
			else
			{
				extraKickableForAngleNoise = 0;
				extraKickableForNoise /= 2;
			}

			if (waitBall.getBodyVec().getMagnitude() <= (bodyKickableArea + extraKickableForNoise + extraKickableForAngleNoise))
			{
				gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 
				extraGTBTime[gtbPointsNumber] = i;
				
				mainGtbTime[gtbPointsNumber] = mainGoToBallTime + 
								extraGTBTime[gtbPointsNumber];
				gtbStatus[gtbPointsNumber] = 1;					
				
				gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);
///*TOF*/				gtbTime[gtbPointsNumber] -= 0;				
				minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();
/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyKickableArea - 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_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 < 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_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 <= WAIT_FOR_BALL_CYCLES; i++)
		{
			extraKickableForNoise = worldModel->getBall().getBodyVec().getMagnitude() * .07;
			float deltaAngleToBallAngle, negBodyAngle;
			negBodyAngle = simBody.getBodyDir() + 180;
			deltaAngleToBallAngle = negBodyAngle - simBall.getVel().getDirection();
			deltaAngleToBallAngle = 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(normalizeAngle(interceptGTBVector.getDirection() - negBodyAngle)) < 2)
			{