block.cpp

mersad源码 03年robocup 季军 可以研究一下大家

/*
 *  Copyright 2002-2004, Mersad Team, Allame 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.
 *
 *  Created by: Meisam Vosoughpour
 *  Released on Friday 1 April 2005 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>

#include <Block.h>
#include <cassert>
#include <Degree.h>
#include <BasicDash.h>
#include <Logger.h>

using namespace std;

float Block::getValue()
{
	LOG << "Block::getValue" << endl;

	float minTurnAngle = 20;
	if (!worldModel->getCurInterCalculate().existFastestPlayer())
	{
		LOG << "BLOCK AD_MIN_VALUE 1" << endl;
		return AD_MIN_VALUE;
	}

	blockCommand = NULL;
	float oppDribbleFixedVel = 1;
	
	Vector oppDribbleVector, opptoMeVector, blockAbsVector, 
			ballVelVector, blockPointToMeVector;

	Point oppDribblePoint = Point(-52.5, 0);
	
	if (worldModel->getBall().getPos().getX() > -35)
	{
		oppDribbleFixedVel = .85;
		oppDribblePoint.y = worldModel->getBall().getPos().getY();
	}
	else
		oppDribbleFixedVel = 1.1;
		
	
	oppDribbleVector.setByPoints(worldModel->getBall().getPos(), 
			oppDribblePoint);

	ballVelVector.setAsPolar(oppDribbleFixedVel, 
						oppDribbleVector.getDirection());
	
	opptoMeVector.setByPoints(worldModel->getBall().getPos(), 
			worldModel->getBody().getPos());

	blockInterCalc.startSession(0, NULL, 
			&worldModel->getCurInterCalculate().getFastestPlayer());
//	blockInterCalc.setLogging(true);
//	if (worldModel->getBallStatus() == BS_KICKABLE_OPP)
//	{
	if (worldModel->getBall().getPos().getX() > -35)
	{
		blockInterCalc.setTmmSeeBallDelay(3);
		blockInterCalc.setTmmSlowDashPower(15);
		blockInterCalc.setTmmDirectDashPower(15);
	}
	else
	{
		blockInterCalc.setTmmSeeBallDelay(2);
		blockInterCalc.setTmmSlowDashPower(20);
		blockInterCalc.setTmmDirectDashPower(20);
	}
//	}
	blockInterCalc.addPlayer(worldModel->getBody());
	blockInterCalc.addPlayer(worldModel->getCurInterCalculate().
			getFastestPlayer(), false, true);
	blockInterCalc.setVirtualBall(worldModel->getBall());		
			
	blockInterCalc.setBlockedBallVel(ballVelVector);		
	blockInterCalc.run();
	
 	while (!blockInterCalc.existFastestPlayer() && 
				ballVelVector.getMagnitude() >= 0)
	{
		ballVelVector.setAsPolar(ballVelVector.getMagnitude() - .05, 
								ballVelVector.getDirection());	
		//	blockInterCalc.setLogging(true);
		if (worldModel->getBallStatus() == BS_KICKABLE_OPP &&
			worldModel->getBall().getPos().getX() > -36)
		{
			blockInterCalc.setTmmSeeBallDelay(2);
			blockInterCalc.setTmmSlowDashPower(20);
			blockInterCalc.setTmmDirectDashPower(20);
		}

		blockInterCalc.setVirtualBall(worldModel->getBall());		
		blockInterCalc.setBlockedBallVel(ballVelVector);		
		blockInterCalc.run();
	}
 	
	if (!blockInterCalc.existFastestPlayer())
	{
		blockCommand = NULL;
		LOG << "BLOCK RETURN AD_MIN_VALUE 2 " << endl;
		return AD_MIN_VALUE;
	} 
	
	blockPoint = blockInterCalc.getFastestInterPlayer().getInterPoint();
	
	blockPointToMeVector.setByPoints(blockPoint, 
				worldModel->getBody().getPos());
	
	blockAbsVector.setAsPolar(blockPointToMeVector.getMagnitude(), 
				Degree::getDeltaAngle(oppDribbleVector.getDirection(), 
				blockPointToMeVector.getDirection()));
	float maxBasicDist = 1.5 + blockPointToMeVector.getMagnitude() / 5.00;
	LOG << "worldModel->getCurInterCalculate().getFastestPlayer().getPos() : "
		<< worldModel->getCurInterCalculate().getFastestPlayer().getPos() << endl;
		
	LOG << "maxBasicDist : " << maxBasicDist << endl;

	Vector changedBlockVector;
	Point changedBlockPoint;
	changedBlockVector.setAsPolar(10, oppDribbleVector.getDirection());
	changedBlockPoint.x = worldModel->getBody().getPos().getX() + changedBlockVector.getX();
	changedBlockPoint.y = worldModel->getBody().getPos().getY() + changedBlockVector.getY();
	LOG << "blockPoint : " << blockPoint << endl;
	LOG << "changedBlockPoint : " << changedBlockPoint << endl;
////////////////////////////////////
	if (worldModel->getBody().getPostNum() <= 5)
	{
		if (blockPoint.x >= 0)
			blockPoint.x = 0;
	}
////////////////THISO
/*	if ((worldModel->getBody().getPostNum() <= 5 || 
		worldModel->getBody().getPostNum() == 8) && 
		worldModel->getBall().getPos().getX() > -34 &&
		worldModel->getBall().getPos().getX() < 0)
	{
		float unitStep = 6.5;
		float deffStep = ((int)(worldModel->getBall().getPos().getX() / 
					unitStep) - 1) * unitStep;
		if (blockPoint.x > deffStep)
			blockPoint.x = deffStep;
	}*/
////////////////THISO



	if ((worldModel->getBody().getPostNum() <= 5 || 
		worldModel->getBody().getPostNum() == 8) &&
		worldModel->getCurInterCalculate().existFastestPlayer())
	{
//		float outsideMaxDelY;
//		float insideMaxDelY;
//		float bodyDeltaX = worldModel->getBody().getPos().getX() - blockPoint.x;
//		float bodyDeltaY = worldModel->getBody().getPos().getY() - blockPoint.y;

		Point localHomePoint = positioning.getHomePoint(worldModel->getBody().getUniNum());

		float deltaX = positioning.getHomePoint(
				worldModel->getBody().getUniNum()).x - blockPoint.x;
	
		float deltaY = positioning.getHomePoint(
				worldModel->getBody().getUniNum()).y - blockPoint.y;
						
		float xDist34 = -4.5;
		if (worldModel->getBall().getPos().getX() > -30)
		{
			if (lastDecide == DT_BLOCK)
				xDist34 = -4.5;
		}
		if (worldModel->getBall().getPos().getX() < -30)
		{
			xDist34 = -2;		
			if (lastDecide == DT_BLOCK)
				xDist34 = -3.5;
		}

		float xDist25 = -4.8;
		if (worldModel->getBall().getPos().getX() > -30)
		{
			if (lastDecide == DT_BLOCK)
				xDist25 = -11;
		}
		if (worldModel->getBall().getPos().getX() < -30)
		{
			xDist25 = -2.5;		
			if (lastDecide == DT_BLOCK)
				xDist25 = -4;
		}
		if (worldModel->getBody().getStamina() < 2000)
			xDist25 = -2;
		if (worldModel->getBody().getStamina() < 1700)
			xDist25 = -1;
		
		float bigDelY34 = 6, smallDelY34 = 6;
		
		if (lastDecide == DT_BLOCK)
		{
			bigDelY34 = 7.2;
			smallDelY34 = 7.2;
		}
		
		LOG << "MEISAM : BLOCK : deltaX , deltaY : " << deltaX << " " << 
				deltaY << endl;
		LOG << "MEISAM : BLOCK : localHomePoint : " <<  localHomePoint << endl;

		switch (worldModel->getBody().getPostNum())
		{
			case 2:
			case 5:
				if (deltaY > 6 || (deltaX < xDist25 && blockPoint.x > 
						worldModel->getBody().getPos().getX()))
					return AD_MIN_VALUE;
				break;
			
			case 3:
			case 4:
				if (!(((fabs(deltaY) < bigDelY34 && fabs(worldModel->getBall().getPos().getY()) < 
					fabs(localHomePoint.y)) || 
					(fabs(deltaY) < smallDelY34 && fabs(worldModel->getBall().getPos().getY()) > 
					fabs(localHomePoint.y))) && 
					(deltaX > xDist34 || blockPoint.x < 
					worldModel->getBody().getPos().getX())))
					return AD_MIN_VALUE;
				else if ((worldModel->getBody().getPos().getX() - 
						worldModel->getBall().getPos().getX()) < -5)
					return AD_MIN_VALUE;	 
					
				break;
			case 8:
				if (worldModel->getBall().getPos().getX() > 5)
				{
					if (!(fabs(deltaY) <= 4 &&
						(fabs(deltaX) <= 2 || 
						blockPoint.x <= worldModel->getBody().getPos().getX())))// || deltaX < -4.5)
						return AD_MIN_VALUE;
					break;
				}
				else if (worldModel->getBall().getPos().getX() > -30)
				{
					LOG << "SALAM Block 1 : Dx,Dy" << deltaX << " " << deltaY << endl;
//
					if (!((fabs(deltaY) <= 4 &&
						(fabs(deltaX) <= 4 || 
						blockPoint.x <= worldModel->getBody().getPos().getX()))
						|| (fabs(deltaY) <= 4 &&
						(deltaX <= 0 || 
						blockPoint.x <= worldModel->getBody().getPos().getX()))))// || deltaX < -4.5)
						return AD_MIN_VALUE;
					
					break;
//
				}
				else
				{
					if (!(fabs(deltaY) <= 4 && 
						(fabs(deltaX) <= 4 || 
						blockPoint.x <= worldModel->getBody().getPos().getX())))// || deltaX < -4.5)
						return AD_MIN_VALUE;
					break;
				}
				break;
		}
	}
////////////////////////////////////

	if (worldModel->getBody().getPostNum() == 6 ||
		worldModel->getBody().getPostNum() == 7)
		if (blockPoint.x < worldModel->getTmmOffsideLine() &&
			worldModel->getBody().getPos().getX() != 
			worldModel->getTmmOffsideLine())
		{
			LOG << "AKHARIN TOF" << endl;
			blockPoint.x = worldModel->getTmmOffsideLine();
//			return AD_MIN_VALUE;
		}	
		
	LOG << "BLOCK fabs(blockAbsVector.getDirection() ( > 110) : " << 
			fabs(blockAbsVector.getDirection()) << endl;

	LOG << "BLOCK fabs(blockAbsVector.getY()) : ( < 8 )" <<
			fabs(blockAbsVector.getY()) << endl; 

	float maxYBlockDist = 5;
	
	maxYBlockDist += (fabs(blockAbsVector.getDirection()) - 130) / 25.00;
	maxYBlockDist = min(maxYBlockDist, float(10));
	
	if (fabs(blockAbsVector.getDirection()) > 155 && 
		fabs(blockAbsVector.getY()) < maxYBlockDist && 
			worldModel->getBall().getPos().getX() > -35)
	{
//		blockPoint = changedBlockPoint;
//		minTurnAngle = 16;		
//		LOG << "blockPoint changed to changedBlockPoint ;)" << endl;
	}
	
	blockCommand = PreciseGotoPoint(blockPoint, .2, worldModel->getBody(), 0,
					 	minTurnAngle).getCommand();
	if (blockCommand == NULL)
		blockCommand = new EmptyCommand();					
	return AD_ALWAYS_RUN_VALUE;
}

Block::Block(const WorldModel *worldModel,
		const Positioning &positioning): AdvancedAction(worldModel),
		positioning(positioning)
{
}

void Block::execute(SayDecisionFlags &sayFlags)
{
	LOG << "Block::execute" << endl;

	command = blockCommand;
}

Block::~Block()
{
}

DecideType Block::getType()
{
	return DT_BLOCK;
}

void Block::giveLastDecide(DecideType lastDecideArg)
{
	lastDecide = lastDecideArg;
}