defense.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 <Logger.h>
#include <Defense.h>
#include <Degree.h>
#include <BasicTurn.h>
#include <BasicDash.h>


using namespace std;

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

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

    defensePoint = positioning.getHomePoint(worldModel->getBody().getUniNum());
//	LOG << "defensePoint1 : " << defensePoint << endl;
    float senseX = 0, senseY = 0; 
	
    if(worldModel->getBody().getPostNum() == 5 || 
		worldModel->getBody().getPostNum() == 2)	
    {
		senseX = 5;
		senseY = 5;
		if (worldModel->getBall().getPos().getX() > -2)
			senseX = 1;
    }
    
	if(worldModel->getBody().getPostNum() == 4 || 
		worldModel->getBody().getPostNum() == 3)	
    {
		senseX = 4;
		senseY = 1.8;
		
		if (worldModel->getBall().getPos().getX() > -20)
		{
			senseX = 5;
			senseY = 4;
		}
    }	
	
		
    if(worldModel->getBody().getPostNum() == 7 ||	
   		worldModel->getBody().getPostNum() == 6)	
    {
		senseX = 13;
		senseY = 13;
		if (worldModel->getPlayMode() == PM_KICK_IN_OPP ||
			worldModel->getPlayMode() == PM_FREE_KICK_OPP ||
			worldModel->getPlayMode() == PM_OFFSIDE ||
			worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||
			worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
			worldModel->getPlayMode() == PM_CATCH_FAULT ||
			worldModel->getPlayMode() == PM_BACK_PASS)
		{
			if (worldModel->getBall().getPos().getX() >= 0)
			{
//				senseX = 10;
//				senseY = 22;
			}
		}
    

    }		
	if (worldModel->getBall().getPos().getX() > 15 && 
			(worldModel->getBody().getPostNum() == 6 ||
			worldModel->getBody().getPostNum() == 7 /*||
			worldModel->getBody().getPostNum() == 9 ||
			worldModel->getBody().getPostNum() == 10 ||
			worldModel->getBody().getPostNum() == 11*/))
		if (worldModel->getPlayMode() == PM_KICK_IN_OPP ||
			worldModel->getPlayMode() == PM_FREE_KICK_OPP ||
			worldModel->getPlayMode() == PM_OFFSIDE ||
			worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||
			worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
			worldModel->getPlayMode() == PM_CATCH_FAULT ||
			worldModel->getPlayMode() == PM_BACK_PASS)
			defensePoint.x -= 8;	

	isPlayOn = true;
	
		if (worldModel->getPlayMode() == PM_KICK_IN_OPP ||
			worldModel->getPlayMode() == PM_FREE_KICK_OPP ||
			worldModel->getPlayMode() == PM_OFFSIDE ||
			worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||
			worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
			worldModel->getPlayMode() == PM_CATCH_FAULT ||
			worldModel->getPlayMode() == PM_BACK_PASS)
			isPlayOn = false;


    if(worldModel->getBody().getPostNum() >= 9)	
    {
		senseX = 13;
		senseY = 13;
    }		
	
    if(worldModel->getBody().getPostNum() == 8)	
    {
		senseX = 8;
		senseY = 4;
		
    }		
    
    if(worldModel->getBody().getPostNum() < 6)
	{
		checkMarkPlayer(defensePoint,senseX, senseY);    
		if(worldModel->getBall().getPos().getX() > -34)
			defensePoint.x = positioning.getHomePoint(worldModel->getBody().
					getUniNum()).x;
	}
		
    if(worldModel->getBody().getPostNum() >= 9 && 
		(worldModel->getPlayMode() == PM_KICK_IN_OPP ||
		worldModel->getPlayMode() == PM_FREE_KICK_OPP ||
		worldModel->getPlayMode() == PM_OFFSIDE ||
		worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||
		worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
		worldModel->getPlayMode() == PM_CATCH_FAULT ||
		worldModel->getPlayMode() == PM_BACK_PASS))
		checkMarkPlayer(defensePoint,senseX, senseY);    
	
    if(worldModel->getBody().getPostNum() == 7 && 
		(worldModel->getBall().getPos().getX() < -30 || 
		(worldModel->getPlayMode() == PM_KICK_IN_OPP ||
		worldModel->getPlayMode() == PM_FREE_KICK_OPP ||
		worldModel->getPlayMode() == PM_OFFSIDE ||
		worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||
		worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
		worldModel->getPlayMode() == PM_CATCH_FAULT ||
		worldModel->getPlayMode() == PM_BACK_PASS)))
		checkMarkPlayer(defensePoint,senseX, senseY);    

    if(worldModel->getBody().getPostNum() == 6 && 
		(worldModel->getBall().getPos().getX() < -30 || 
		(worldModel->getPlayMode() == PM_KICK_IN_OPP ||
		worldModel->getPlayMode() == PM_FREE_KICK_OPP ||
		worldModel->getPlayMode() == PM_OFFSIDE ||
		worldModel->getPlayMode() == PM_CORNER_KICK_OPP ||
		worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
		worldModel->getPlayMode() == PM_CATCH_FAULT ||
		worldModel->getPlayMode() == PM_BACK_PASS)))
		checkMarkPlayer(defensePoint,senseX, senseY);    
    
	if(worldModel->getBody().getPostNum() == 8 && 
		worldModel->getBall().getPos().getX() < 10)
	{
		checkMarkPlayer(defensePoint,senseX, senseY);    
		if(worldModel->getBall().getPos().getX() > -34)
			defensePoint.x = positioning.getHomePoint(worldModel->getBody().
					getUniNum()).x;
	}	

//	LOG << "defensePoint2 : " << defensePoint << endl;
	defenseCommand = positioning.getPositioningCommand(defensePoint);
//    checkForGoal(defensePoint);
//	LOG << "defensePoint3 : " << defensePoint << endl;
	return AD_NORMAL_VALUE;
}

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

	command = defenseCommand;
}

DecideType Defense::getType()
{
	return DT_DEFENSE;
}

void Defense::checkGoalDist(Point &homePoint)
{
    Point ballPoint,tirPoint;

    if(worldModel->getBody().getPostNum() == 5)
    {
		tirPoint.x = -52.5;
		tirPoint.y = 5;
    }
	
    if(worldModel->getBody().getPostNum() == 2)
    {
		tirPoint.x = -52.5;
		tirPoint.y = -5;
    }

    ballPoint.x = worldModel->getBall().getPos().getX();
    ballPoint.y = worldModel->getBall().getPos().getY();

    float ballToGoalDist, playerToGoalDist, homeToGoalDist; 

    ballToGoalDist = hypot(ballPoint.x - tirPoint.x, ballPoint.y - tirPoint.y);
    playerToGoalDist = ballToGoalDist * .5;
    homeToGoalDist = hypot(homePoint.x - tirPoint.x, homePoint.y - tirPoint.y);

    if((ballToGoalDist < 20 || worldModel->getBall().getPos().getX() < -30) 
			&& homeToGoalDist > playerToGoalDist)
    {
		Vector tirVector;
		tirVector.setByPoints(tirPoint, homePoint);
		tirVector.setAsPolar(playerToGoalDist, tirVector.getDirection());
		homePoint.x = tirPoint.x + tirVector.getX();
		homePoint.y = tirPoint.y + tirVector.getY();
    }
}


int Defense::checkMarkPlayer(Point &homePoint, float senseX, float senseY)
{
    Point markPoint[20];
    int bestMarkPoint = 0;
    float distsToMarkPoint[20];
    int markPlayersNum = 0;
    Point playerPoint;
    
	for (int j = 0; j < FULL_PLAYERS_NUM; j++) 
		if (worldModel->getFullPlayer(TID_OPPONENT, j).isValid())
		{		
			playerPoint.x = worldModel->getFullPlayer(TID_OPPONENT, j).getPos().getX();
		    playerPoint.y = worldModel->getFullPlayer(TID_OPPONENT, j).getPos().getY();
	    
			float deltaY, deltaX;
	    
			deltaX = fabs(playerPoint.x - homePoint.x);
		    deltaY = fabs(playerPoint.y - homePoint.y);
		    
			if(deltaX < senseX && deltaY < senseY)
		    {
				markPoint[markPlayersNum] = makeMarkPoint(playerPoint);
				
				float playerDist = hypot(markPoint[markPlayersNum].x - worldModel->getBody().getPos().getX(), markPoint[markPlayersNum].y - worldModel->getBody().getPos().getY());
				float homeDist = hypot(homePoint.x - markPoint[markPlayersNum].x, homePoint.y - markPoint[markPlayersNum].y);	
				distsToMarkPoint[markPlayersNum] = playerDist + homeDist;

				markPlayersNum++;
		    }
		}    

	for (int j = 0; j < HALF_PLAYERS_NUM; j++) 
		if (worldModel->getHalfPlayer(TID_OPPONENT, j).isValid())
		{		
			playerPoint.x = worldModel->getHalfPlayer(TID_OPPONENT, j).getPos().getX();
		    playerPoint.y = worldModel->getHalfPlayer(TID_OPPONENT, j).getPos().getY();
	    
			float deltaY, deltaX;
	    
			deltaX = fabs(playerPoint.x - homePoint.x);
		    deltaY = fabs(playerPoint.y - homePoint.y);
		    
			if(deltaX < senseX && deltaY < senseY)
		    {
				markPoint[markPlayersNum] = makeMarkPoint(playerPoint);
				
				float playerDist = hypot(markPoint[markPlayersNum].x - worldModel->getBody().getPos().getX(), markPoint[markPlayersNum].y - worldModel->getBody().getPos().getY());
				float homeDist = hypot(homePoint.x - markPoint[markPlayersNum].x, homePoint.y - markPoint[markPlayersNum].y);	
				distsToMarkPoint[markPlayersNum] = playerDist + homeDist;

				markPlayersNum++;
		    }
		}    

    for(int j = 0; j < QUARTER_PLAYERS_NUM; j++)
        if (worldModel->getQuarterPlayer(j).isValid())
		{		
			playerPoint.x = worldModel->getQuarterPlayer(j).getPos().getX();
		    playerPoint.y = worldModel->getQuarterPlayer(j).getPos().getY();
	    
			float deltaY, deltaX;
	    
			deltaX = fabs(playerPoint.x - homePoint.x);
		    deltaY = fabs(playerPoint.y - homePoint.y);
		    
			if(deltaX < senseX && deltaY < senseY)
		    {
				markPoint[markPlayersNum] = makeMarkPoint(playerPoint);
				
				float playerDist = hypot(markPoint[markPlayersNum].x - worldModel->getBody().getPos().getX(), markPoint[markPlayersNum].y - worldModel->getBody().getPos().getY());
				float homeDist = hypot(homePoint.x - markPoint[markPlayersNum].x, homePoint.y - markPoint[markPlayersNum].y);	
				distsToMarkPoint[markPlayersNum] = playerDist + homeDist;

				markPlayersNum++;
		    }
		}    

    if(markPlayersNum == 0)
    {
		LOG << "I did not mark any player" << endl;
		return 0;
    }
	
    for(int i = 0; i < markPlayersNum; i++)
		if(distsToMarkPoint[bestMarkPoint] > distsToMarkPoint[i])
	    	bestMarkPoint = i;

    LOG << "I am marking a point that is : " << markPoint[bestMarkPoint] << endl;
	homePoint = markPoint[bestMarkPoint]; 
	return 0;
}

Point Defense::makeMarkPoint(Point playerPoint)
{
    Point markPoint, markPoint1, markPoint2;
	float plusX;
    
	plusX = 2;
    plusX += ((hypot(playerPoint.x - worldModel->getBody().getPos().getX(), 
			playerPoint.y - worldModel->getBody().getPos().getY())) / 10);
    
	if(plusX > 3)
		plusX = 3;
		
//making markPoint1    
	markPoint1.x = playerPoint.x - plusX;
    markPoint1.y = playerPoint.y;

//making markPoint2
	Vector markVector;
	markVector.setByPoints(playerPoint, worldModel->getBall().getPos());
	markVector.setAsPolar(3, markVector.getDirection());

	markPoint2.x = playerPoint.x + markVector.getX();
	markPoint2.y = playerPoint.y + markVector.getY();


    if (worldModel->getBall().getPos().getX() < 
			playerPoint.x && markPoint.x < -43)
    {
//Algorithm1 remarked.	
/*		float markDist1, markDist2;

		markDist1 = hypot(BasicPos[worldModel->getBody().getUniNum() - 1].x - markPoint1.x, BasicPos[worldModel->getBody().getUniNum() - 1].y - markPoint1.y);
		markDist2 = hypot(BasicPos[worldModel->getBody().getUniNum() - 1].x - markPoint2.x, BasicPos[worldModel->getBody().getUniNum() - 1].y - markPoint2.y);

		if(markDist1 < markDist2)
		    markPoint = markPoint1;
		else
	    	markPoint = markPoint2;   */

//Algorithm2 is used
		markPoint.x = (markPoint2.x - markPoint1.x) * .8 + markPoint1.x;
		markPoint.y = (markPoint2.y - markPoint1.y) * .8 + markPoint1.y;
	}
	else
		markPoint = markPoint1;
    
	if (!isPlayOn && worldModel->getBall().getPos().getX() > -10 &&
		worldModel->getBody().getPostNum() >= 6 && 
		worldModel->getBody().getPostNum() != 8)
	{
		markPoint = markPoint2;
	}		
	
	return markPoint;
}


void Defense::checkForGoal(Point &homePoint)
{
    Point tirPoint;
    float currentDir;
	
    if(worldModel->getBody().getPostNum() == 5)
    {
		if(fabs(positioning.getHomePoint(worldModel->getBody().getUniNum()).x + 52.5) < 11 && 
				fabs(positioning.getHomePoint(worldModel->getBody().getUniNum()).y - 7) < 7)
		{
	    	tirPoint.x = -51;
	    	
			if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 
					tirPoint.x > worldModel->getBody().getPos().getX())
				tirPoint.x = worldModel->getBody().getPos().getX();
	    	
			float A = hypot(worldModel->getBall().getPos().getX() + 52.5, 
						worldModel->getBall().getPos().getY() - 7) / 
						hypot(worldModel->getBall().getPos().getX() + 52.5, 
						worldModel->getBall().getPos().getY() - 3);
						
	   		A = A * A * A;
	   		
			float B = 4;
	    	float M = (A * B) / (A + 1);
//	    	float N = B - M;
	    	
			tirPoint.y = 7 - M;
	    	
			if(worldModel->getBall().getPos().getY() >= tirPoint.y)
				currentDir = 90;
	    	else
				currentDir = -90;

			if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 
						tirPoint.x > worldModel->getBody().getPos().getX())
				tirPoint.x = worldModel->getBody().getPos().getX();

			homePoint = tirPoint;
			gotoTir(tirPoint, currentDir);
		}
    }
    
	if(worldModel->getBody().getPostNum() == 2)
    {
		if(fabs(positioning.getHomePoint(worldModel->getBody().getUniNum()).x + 52.5) < 13 && 
			fabs(positioning.getHomePoint(worldModel->getBody().getUniNum()).y + 7) < 7)
		{
		    tirPoint.x = -51;
			
		    if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 
					tirPoint.x > worldModel->getBody().getPos().getX())
				tirPoint.x = worldModel->getBody().getPos().getX();
		    
			float A = hypot(worldModel->getBall().getPos().getX() + 52.5, 
						worldModel->getBall().getPos().getY() + 7) / 
						hypot(worldModel->getBall().getPos().getX() + 52.5, 
						worldModel->getBall().getPos().getY() + 3);
		    
			A = A * A * A;
		    
			float B = 4;
		    float M = (A * B) / (A + 1);
//		    float N = B - M;
		    
			tirPoint.y = -7 + M;
		    
			if(worldModel->getBall().getPos().getY() >= tirPoint.y)
				currentDir = 90;
	    	else
				currentDir = -90;
	    	
	    	if(fabs(tirPoint.x - worldModel->getBody().getPos().getX()) < 1 && 
					tirPoint.x > worldModel->getBody().getPos().getX())
				tirPoint.x = worldModel->getBody().getPos().getX();
		
			homePoint = tirPoint;
			gotoTir(tirPoint, currentDir);
		}	
    }
}

void Defense::gotoTir(Point tirPoint, float currentAngle)
{
    Vector lenVector;
    
    lenVector.setByPoints(worldModel->getBody().getPos(), tirPoint);
    
    if (lenVector.getMagnitude() <= .2)
    {
        float deltaAngle = currentAngle - worldModel->getBody().getBodyDir();
        deltaAngle = Degree::normalizeAngle(deltaAngle);
		defenseCommand = TurnAngle(deltaAngle,worldModel->getBody()).getCommand();
    }
    else
    {
		if(lenVector.getMagnitude() >= 6)
    		defenseCommand = DashNormalGotoPoint(tirPoint, 1.5, worldModel->getBody()).getCommand();
		else
    		defenseCommand = PreciseGotoPoint(tirPoint, .2, worldModel->getBody(), 1).getCommand();
    }
}

Point Defense::getHomePoint(int playerUniNum)
{
	return defensePoint;
}