goaliepositioning.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 <BasicTurn.h>
#include <BasicDash.h>
#include <Basics.h>
#include <GoaliePositioning.h>


using namespace std;

GoaliePositioning::GoaliePositioning(const WorldModel *worldModel):
		AdvancedAction(worldModel)
{
}

void GoaliePositioning::setGoalieParams()
{
    Point ballPoint;
    float changeXDist = 1;
    float changeYDistBadAngle = 1;
    float minGoalieX = -48.5;
    float virtualMinGoalieX;
//  	float overLoadDistByNoTurn;
// 		float overLoadDistByTurn;
    float virtualGoalieX, virtualGoalieYIn52, 
		virtualGoalieYIn44, virtualGoalieYInNow;
    
	Line midAngleLine, upWallLine, downWallLine, directWallLine; 
    //KOMAK
		/*KOMAK*/ float goalieXHome = -47;
	//KOMAK
	if (worldModel->getPlayMode() == PM_PENALTY_SETUP ||
		worldModel->getPlayMode() == PM_PENALTY_SETUP_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY)
	{
		goalieXHome = -40;
		changeXDist = 2;
	}	
	else
	{
//	 LATEST TRICK	
	if (config["Agent"]["Public"]["LatestTrick"].asBool() && 
		worldModel->getBall().getPos().getX() > -3)
	{
		if (worldModel->getBall().getPos().getX() > -34 &&
			worldModel->getBall().getPos().getX() < -15)
			goalieXHome = Basics::reRate(worldModel->getBall().getPos().getX(),
					-25, -15, -47, -38);
		
		if (worldModel->getBall().getPos().getX() > -15 &&
			worldModel->getBall().getPos().getX() < 0)
			goalieXHome = Basics::reRate(worldModel->getBall().getPos().getX(),
					-15, 0, -38, -20);

	
		if (worldModel->getBall().getPos().getX() >= 0)
			goalieXHome = Basics::reRate(worldModel->getBall().getPos().getX(),
					0, 54, -24, -15);
		if (goalieXHome > -32)
			goalieXHome = -32;
	}
//	LATEST TRICK 				
	}
	
    virtualGoalieX = goalieXHome;
    virtualGoalieYIn52 = 6;
    virtualGoalieYIn44 = 6;

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

    if(ballPoint.x < -52.4)
		ballPoint.x = -52.4;

    virtualMinGoalieX = -52.5 + float((ballPoint.x - (-52.5)) / 2.8);   
    
    if(worldModel->getBall().getPos().getX() < -25 && 
		worldModel->getBody().getPos().getX() < virtualGoalieX)
    {
		if (worldModel->getBody().getPos().getX() > virtualMinGoalieX)
	   		virtualGoalieX = worldModel->getBody().getPos().getX(); 
		else if (virtualMinGoalieX < virtualGoalieX) 
			virtualGoalieX = virtualMinGoalieX;
    }

    if(virtualGoalieX < minGoalieX)
		virtualGoalieX = minGoalieX;

    if(fabs(virtualGoalieX - worldModel->getBody().getPos().getX()) 
		< changeXDist)
		virtualGoalieX = worldModel->getBody().getPos().getX();
    
    virtualGoalieYInNow = virtualGoalieYIn44 + ((-44 - 
							virtualGoalieX) * (virtualGoalieYIn52 - 
							virtualGoalieYIn44)) / (-44 - (-52.5));	 

    Point tirUpPoint, tirDownPoint;
    Vector tirUpVector, tirDownVector, midAngleVector;
    float deltaAngle, betweenAngle;
    
    tirUpPoint.x = tirDownPoint.x = -52.5;
    tirUpPoint.y = 7;
    tirDownPoint.y = -7;
    
    tirUpVector.setByPoints(ballPoint, tirUpPoint);
    tirDownVector.setByPoints(ballPoint, tirDownPoint);
    
    deltaAngle = tirUpVector.getDirection() - tirDownVector.getDirection();
    deltaAngle = Degree::normalizeAngle(deltaAngle);

    deltaAngle = fabs(deltaAngle);
    deltaAngle /= 2.00;
    
    betweenAngle = tirUpVector.getDirection() + deltaAngle;
    betweenAngle = Degree::normalizeAngle(betweenAngle);
    
    midAngleVector.setAsPolar(1, betweenAngle);
    midAngleLine.setBySourceDir(ballPoint, midAngleVector.getDirection());
    
    upWallLine.setByPoints(Point(virtualGoalieX, virtualGoalieYInNow), 
							Point(-52.5, virtualGoalieYIn52));
						
    downWallLine.setByPoints(Point(virtualGoalieX, -virtualGoalieYInNow), 
							Point(-52.5, -virtualGoalieYIn52));
							
    directWallLine.setByPoints(Point(virtualGoalieX, -1), 
								Point(virtualGoalieX, 1));

	LOG << "directWallLine : a, b, c = " << directWallLine.getA() << "," << directWallLine.getB() << "," << directWallLine.getC() << endl;
	LOG << "midAngleLine : a, b, c = " << midAngleLine.getA() << "," << midAngleLine.getB() << "," << midAngleLine.getC() << endl;
	LOG << "midAngleLine : direction = " << midAngleLine.getDirection() << endl;
	LOG << "virtualGoalieX = " << virtualGoalieX << endl;

    midAngleLine.getLineIntersect(directWallLine, goalieHomePoint);

    LOG << "goalieHomePoint 1 : " << goalieHomePoint << endl;
    
    if(goalieHomePoint.y > virtualGoalieYInNow)
    {
		midAngleLine.getLineIntersect(upWallLine, goalieHomePoint);
    	LOG << "goalieHomePoint 2 : " << goalieHomePoint << endl;
    }
    else if(goalieHomePoint.y < (-virtualGoalieYInNow))
	{
	    midAngleLine.getLineIntersect(downWallLine, goalieHomePoint);
    	LOG << "goalieHomePoint 3 : " << goalieHomePoint << endl;
	}

    mainIdealBetweenAngle = midAngleVector.getDirection(); 
    
    Vector goalieHomeVector;
    goalieHomeVector.setByPoints(worldModel->getBody().getPos(), 
								goalieHomePoint);    
    
	float localDeltaAngle = goalieHomeVector.getDirection() - 
							worldModel->getBody().getBodyDir();
							
    localDeltaAngle = Degree::normalizeAngle(localDeltaAngle);
    localDeltaAngle = fabs(localDeltaAngle);
    
	if((localDeltaAngle >= 15 || localDeltaAngle >= 165) && 
		fabs(goalieHomePoint.y - worldModel->getBody().getPos().getY()) <= 
		changeYDistBadAngle)
		goalieHomePoint.y = worldModel->getBody().getPos().getY();

    LOG << "goalieHomePoint 4 : " << goalieHomePoint << endl;

    float idealBodyAngle = mainIdealBetweenAngle - 90;
    idealBodyAngle = Degree::normalizeAngle(idealBodyAngle);

    Line idealAngleLine, ballToHomeLine;
    Point bodyPoint;
	
    bodyPoint.x = worldModel->getBody().getPos().getX();
    bodyPoint.y = worldModel->getBody().getPos().getY();
    
	idealAngleLine.setBySourceDir(bodyPoint, idealBodyAngle);

    Line tirUpLine, tirDownLine;
    tirUpLine.setByPoints(ballPoint, tirUpPoint);
    tirDownLine.setByPoints(ballPoint, tirDownPoint);

    LOG << "goalieHomePoint 5 : " << goalieHomePoint << endl;

    float miniChangeDist = .3;
	
    if(worldModel->getBall().getBodyVec().getMagnitude() >= 10)
		miniChangeDist += (worldModel->getBall().getBodyVec().getMagnitude() 
							- 10) * .05;
    
	if(worldModel->getBall().getBodyVec().getMagnitude() >= 25)
		miniChangeDist = 1.2;
    
	if (worldModel->getPlayMode() != PM_PLAY_ON)
    	miniChangeDist = 5;    
    
	if(mainIdealBetweenAngle > -122 && mainIdealBetweenAngle < 0)
    {
		goalieHomePoint = shootToTirSimulate(tirUpPoint, goalieHomePoint, 
							idealBodyAngle, .00/*firstDashLength*/, 
							6/*maxChangeDistance*/,miniChangeDist);
							
    	LOG << "goalieHomePoint 6 : " << goalieHomePoint << endl;
    }
	
    if(mainIdealBetweenAngle < 122 && mainIdealBetweenAngle > 0)
    {
		goalieHomePoint = shootToTirSimulate(tirDownPoint, goalieHomePoint, 
							idealBodyAngle, .00/*firstDashLength*/, 
							6/*maxChangeDistance*/,miniChangeDist);
							
    	LOG << "goalieHomePoint 7 : " << goalieHomePoint << endl;
    }    

    if(hypot(goalieHomePoint.x - worldModel->getBody().getPos().getX(), 
		goalieHomePoint.y - worldModel->getBody().getPos().getY()) < 3)
    {
    	if(mainIdealBetweenAngle < -122 && mainIdealBetweenAngle > -160) 
    	{
			Point tempSource;
			idealAngleLine.getLineIntersect(tirUpLine, tempSource);
			goalieHomePoint = shootToTirSimulate(tempSource, goalieHomePoint, 
								idealBodyAngle, .00/*firstDashLength*/, 
								7/*maxChangeDistance*/, .3);
								
 		   	LOG << "goalieHomePoint 8 : " << goalieHomePoint << endl;
    	}
		
	    if(mainIdealBetweenAngle > 124 && mainIdealBetweenAngle < 160) 
	    {
			Point tempSource;
			idealAngleLine.getLineIntersect(tirDownLine, tempSource);
			goalieHomePoint = shootToTirSimulate(tempSource, goalieHomePoint, 
								idealBodyAngle, .00/*firstDashLength*/, 
								7/*maxChangeDistance*/, .3);
								
  		  	LOG << "goalieHomePoint 9 : " << goalieHomePoint << endl;
    	}
    }

    LOG << "goalieHomePoint 10 : " << goalieHomePoint << endl;
    
    ballToHomeLine.setByPoints(goalieHomePoint, ballPoint);

    float permitedDist = 1.2;
    
	if(hypot(goalieHomePoint.x - worldModel->getBody().getPos().getX(), 
		goalieHomePoint.y - worldModel->getBody().getPos().getY()) < permitedDist)
		idealAngleLine.getLineIntersect(ballToHomeLine, goalieHomePoint);

   	LOG << "goalieHomePoint 11 : " << goalieHomePoint << endl;

    if(goalieHomePoint.x < -50.5 )
		goalieHomePoint.x = -50.5;

   	LOG << "goalieHomePoint 12 : " << goalieHomePoint << endl;
}

void GoaliePositioning::setGoalieParamsLineMode()
{
    float goalieXHomeLineMode = -47;
    float goalieXHomeEmg = -49.5;
    float mainGoalieXHomeLM;
	float minGoalieXLM = -50;
	float virtualMinGoalieXLM;

	if (worldModel->getPlayMode() == PM_PENALTY_SETUP ||
		worldModel->getPlayMode() == PM_PENALTY_SETUP_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY)
	{
	}
	else
	{
/////////
//	LATEST TRICK
	if (config["Agent"]["Public"]["LatestTrick"].asBool() &&
		worldModel->getBall().getPos().getX() > -3)
	{
		if (worldModel->getBall().getPos().getX() > -34 &&
			worldModel->getBall().getPos().getX() < -15)
			goalieXHomeLineMode = Basics::reRate(worldModel->getBall().getPos().getX(),
					-25, -15, -47, -38);
		
		if (worldModel->getBall().getPos().getX() > -15 &&
			worldModel->getBall().getPos().getX() < 0)
			goalieXHomeLineMode = Basics::reRate(worldModel->getBall().getPos().getX(),
					-15, 0, -38, -20);

	
		if (worldModel->getBall().getPos().getX() >= 0)
			goalieXHomeLineMode = Basics::reRate(worldModel->getBall().getPos().getX(),
					0, 54, -24, -15);
		if (goalieXHomeLineMode > -32)
			goalieXHomeLineMode = -32;
	}
//	LATEST TRICK 

//////////	
	}	
    Vector upTirToBall, downTirToBall, goalToBall;
    Point ballPoint;
    
    ballPoint.x = worldModel->getBall().getPos().getX() + 
					worldModel->getBall().getVel().getX();
					
    ballPoint.y = worldModel->getBall().getPos().getY() + 
					worldModel->getBall().getVel().getY();

    mainGoalieXHomeLM = goalieXHomeLineMode;
	virtualMinGoalieXLM = -52.5 + float((ballPoint.x - (-52.5)) / 2.8);   
	minGoalieXLM = max(minGoalieXLM, virtualMinGoalieXLM);

	if (worldModel->getBody().getPos().getX() < mainGoalieXHomeLM)
	{
		if (worldModel->getBody().getPos().getX() > minGoalieXLM)
			mainGoalieXHomeLM = worldModel->getBody().getPos().getX();	
	}
	
    upTirToBall.setByPoints(Point(-52.5, 7), ballPoint);
    downTirToBall.setByPoints(Point(-52.5, -7), ballPoint);
    goalToBall.setByPoints(Point(-52.5, 0), ballPoint);
	

    float deltaAngleToUpTir = upTirToBall.getDirection() - 90;
    float deltaAngleToDownTir = downTirToBall.getDirection() - (-90);
    float deltaAngleToGoal; 
   	if (ballPoint.y > 0)
    	deltaAngleToGoal = goalToBall.getDirection() - 90;
	else
    	deltaAngleToGoal = goalToBall.getDirection() - (-90);
		
	
    deltaAngleToUpTir = Degree::normalizeAngle(deltaAngleToUpTir);
    deltaAngleToDownTir = Degree::normalizeAngle(deltaAngleToDownTir);
    deltaAngleToGoal = Degree::normalizeAngle(deltaAngleToGoal);
	
    deltaAngleToUpTir = fabs(deltaAngleToUpTir);
    deltaAngleToDownTir = fabs(deltaAngleToDownTir);
    deltaAngleToGoal = fabs(deltaAngleToGoal);

	LOG << "Goalie LOG 1 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;

    if(worldModel->getBall().getPos().getY() > 0 && 
		deltaAngleToUpTir < 45 && upTirToBall.getMagnitude() < 15)
		{
			mainGoalieXHomeLM = goalieXHomeEmg;
			LOG << "GOALIE Change Function 1" << endl;
			setGoalieParams();
			return;
		}	

	LOG << "Goalie LOG 2 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;
		
    if(worldModel->getBall().getPos().getY() < 0 && 
		deltaAngleToDownTir < 45 && downTirToBall.getMagnitude() < 15)
		{
			mainGoalieXHomeLM = goalieXHomeEmg;
			LOG << "GOALIE Change Function 2" << endl;
			setGoalieParams();
			return;
		}
	LOG << "Goalie LOG 3 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;

    LOG << "deltaAngleToGoal : " << deltaAngleToGoal << endl;
    LOG << "goalToBall.Length : " << goalToBall.getMagnitude() << endl;