goaliepositioning.cpp

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

	if(deltaAngleToGoal < 45 && goalToBall.getMagnitude() < 30)
		{
			mainGoalieXHomeLM = goalieXHomeEmg;
			LOG << "GOALIE Change Function 3" << endl;
			setGoalieParams();
			return;
		}


    if(goalieXHomeLineMode == goalieXHomeEmg)
	{
		if (fabs(worldModel->getBody().getPos().getX() - 
			mainGoalieXHomeLM) < 1)
		{
	    	mainGoalieXHomeLM = worldModel->getBody().getPos().getX();
	LOG << "Goalie LOG 4 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;
		}
	}
    else if(mainGoalieXHomeLM == goalieXHomeLineMode)
	{
		if(fabs(worldModel->getBody().getPos().getX() - mainGoalieXHomeLM) < 1)
	 	{
	    	mainGoalieXHomeLM = worldModel->getBody().getPos().getX();
	LOG << "Goalie LOG 5 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;
		}
	}
    else if(mainGoalieXHomeLM == goalieXHomeEmg)
    {
		if(fabs(worldModel->getBody().getPos().getX() - mainGoalieXHomeLM) < .5)
	    	mainGoalieXHomeLM = worldModel->getBody().getPos().getX();

	LOG << "Goalie LOG 6 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;
		
		if(fabs(worldModel->getBody().getPos().getX() - mainGoalieXHomeLM) < 1 
			&& worldModel->getBody().getPos().getX() < mainGoalieXHomeLM)
	    	mainGoalieXHomeLM = worldModel->getBody().getPos().getX();

	LOG << "Goalie LOG 7 : " << mainGoalieXHomeLM << " " << goalieXHomeLineMode 
		<< " " << goalieXHomeEmg << endl;
    }	


    Line goalLine, midAngleLine;
    goalLine.setByPoints(Point(mainGoalieXHomeLM, 7), Point(mainGoalieXHomeLM, -7));

    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;
    
    betweenAngle = tirUpVector.getDirection() + deltaAngle;
    betweenAngle = Degree::normalizeAngle(betweenAngle);
    
    midAngleVector.setAsPolar(1, betweenAngle);
    midAngleLine.setBySourceDir(ballPoint, midAngleVector.getDirection());
    
    midAngleLine.getLineIntersect(goalLine, goalieHomePoint);
  
    if(goalieHomePoint.y > 6)
		goalieHomePoint.y = 6;
    
	if(goalieHomePoint.y < -6)
		goalieHomePoint.y = -6;
    
    if(worldModel->getBall().getPos().getY() >= 0)
		mainIdealBetweenAngle = 180;
    
	if(worldModel->getBall().getPos().getY() < 0)
		mainIdealBetweenAngle = 0;
}

Point GoaliePositioning::shootToTirSimulate(Point sourcePoint, 
						Point beforePoint, float idealBodyAngle, 
						float firstDashLength, float maxChangeDistance, 
						float miniChangeDist)
{
    if(fabs(idealBodyAngle) > 90)
    {
		idealBodyAngle += 180;
		idealBodyAngle = Degree::normalizeAngle(idealBodyAngle);
    } 
    
    Body simBody;
    Ball simBall;
    
	simBody = worldModel->getBody();
    simBall = worldModel->getBall();
    
	Point startPoint;
    Vector actionVector;
    float dashPower;
    
    dashPower = 100;
    if(simBody.getStamina() < 1400)
		dashPower = worldModel->getBody().getStaminaIncMax();

	Command *dashCommand = new DashCommand(AT_GOALIE_POSITIONING, dashPower);

    actionVector.setAsPolar(firstDashLength, idealBodyAngle);    

    startPoint.x = sourcePoint.x + actionVector.getX();
    startPoint.y = sourcePoint.y + actionVector.getY();

    simBody.setPos().setAsCartesian(startPoint.x, startPoint.y);
    simBody.setVel().setAsCartesian(0, 0);
    
	simBody.setBodyDir(idealBodyAngle);
    
    Vector shootVector;
    shootVector.setByPoints(simBall.getPos(), sourcePoint); 
    
	simBall.setVel().setAsPolar(2.65, shootVector.getDirection());
    simBall.simulateByDynamics(simBody);
    
//pak nashavad    float distToSourcePoint = hypot(simBall.getPos().getX() - sourcePoint.x, 
//pak nashavad								simBall.getPos().getY() - sourcePoint.y);
    
    while(simBall.getPos().getX() > sourcePoint.x && 
			hypot(simBody.getPos().getX() - sourcePoint.x, 
			simBody.getPos().getY() - sourcePoint.y) < maxChangeDistance) 
    {
		simBall.simulateByDynamics(simBody);
		simBody.simulateByAction(dashCommand);
		simBody.simulateByDynamics();
    }
    
    Point targetPoint;
    targetPoint.x = simBody.getPos().getX();
    targetPoint.y = simBody.getPos().getY();

    float curDistance = hypot(targetPoint.x - 
			worldModel->getBody().getPos().getX(), 
			targetPoint.y - worldModel->getBody().getPos().getY());

    if(curDistance < miniChangeDist)
    {
		targetPoint.x = worldModel->getBody().getPos().getX();
		targetPoint.y = worldModel->getBody().getPos().getY();
    }
	delete dashCommand;
//	if(hypot(targetPoint.x - sourcePoint.x, targetPoint.y - sourcePoint.y) < 
//		hypot(BeforePoint.x - sourcePoint.x, BeforePoint.y - sourcePoint.y))
//		targetPoint = BeforePoint;
    
    return targetPoint;     
}


float GoaliePositioning::getValue(const Library &library)
{
	if (worldModel->getPlayMode() == PM_PENALTY_SETUP ||
		worldModel->getPlayMode() == PM_PENALTY_SETUP_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY)
    	setGoalieParams();
 	else
	    setGoalieParamsLineMode();
    
    float idealBodyAngle = 0;
//pak nashavad	float idealGTPAngle;

    if(worldModel->getBall().getPos().getY() > 0)
    {
		idealBodyAngle = mainIdealBetweenAngle - 90;
		idealBodyAngle = Degree::normalizeAngle(idealBodyAngle);
    }

    if(worldModel->getBall().getPos().getY() <= 0)
    {
		idealBodyAngle = mainIdealBetweenAngle - 90;
		idealBodyAngle = Degree::normalizeAngle(idealBodyAngle);
    }

    float actionDistMin = .08;
    float actionDistMax = 1.5;

    float distMin = 15, distMax = 35;
    float actionDist;
    float playerToBallDist = hypot(worldModel->getBody().getPos().getX() - 
			worldModel->getBall().getPos().getX(), 
			worldModel->getBody().getPos().getY() - 
			worldModel->getBall().getPos().getY());

    if (playerToBallDist < distMin)
		actionDist = actionDistMin;
    else if(playerToBallDist > distMax)
		actionDist = actionDistMax;
    else
    {
		float rangeDistToBall = distMax - distMin;
		float rangeActionDist = actionDistMax - actionDistMin;
		actionDist = actionDistMin + ((playerToBallDist - distMin) / 
						rangeDistToBall) * rangeActionDist;
    }	

    if (worldModel->getBody().getStamina() < 2000)
	{
		actionDist += 1.3;
		actionDist *= 1.2;
	}
	
	if (worldModel->getPlayMode() != PM_PLAY_ON)
	{
		actionDist += 1.3;
		actionDist *= 1.2;
	}
    
	if (worldModel->getPlayMode() == PM_PENALTY_SETUP ||
		worldModel->getPlayMode() == PM_PENALTY_SETUP_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY_OPP ||
		worldModel->getPlayMode() == PM_PENALTY_READY)
		actionDist = .2;	
	
	float distToHome = hypot(worldModel->getBody().getPos().getX() - 
					goalieHomePoint.x, worldModel->getBody().getPos().getY() - 
					goalieHomePoint.y);
	float distToGoal = hypot(worldModel->getBody().getPos().getX() - 
					(-52.5), worldModel->getBody().getPos().getY() - 0);
	if (distToHome >= actionDist)
    {	
		if (distToHome >= 10 || distToGoal > 15)
		{
			if (worldModel->getPlayMode() == PM_FREE_KICK_OPP||
				worldModel->getPlayMode() == PM_OFFSIDE ||
				worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
				worldModel->getPlayMode() == PM_CATCH_FAULT ||
				worldModel->getPlayMode() == PM_BACK_PASS)
			{
				ForbiddenAreaGotoPoint doGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, worldModel->getBall().
					getPos(), 9.15, worldModel->getBody());
				if (doGotoPoint.getStatus() == 0)
				{
					if (doGotoPoint.getCommand())
						delete doGotoPoint.getCommand();
					goaliePositioningCommand = TurnToPoint(AT_GOALIE_POSITIONING, worldModel->getBall().getPos(),
						worldModel->getBody(), 5).getCommand();
				}
				else
					goaliePositioningCommand = doGotoPoint.getCommand();
			}
			else
				goaliePositioningCommand = PreciseGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, actionDist, 
					worldModel->getBody(), 0).getCommand();
		}
		else
		{
			if (worldModel->getPlayMode() == PM_FREE_KICK_OPP||
				worldModel->getPlayMode() == PM_OFFSIDE ||
				worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
				worldModel->getPlayMode() == PM_CATCH_FAULT ||
				worldModel->getPlayMode() == PM_BACK_PASS)
			{
				ForbiddenAreaGotoPoint doGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, worldModel->getBall().
					getPos(), 9.15, worldModel->getBody());
				if (doGotoPoint.getStatus() == 0)
				{
					if (doGotoPoint.getCommand())
						delete doGotoPoint.getCommand();
					goaliePositioningCommand = TurnToPoint(AT_GOALIE_POSITIONING, worldModel->getBall().getPos(),
						worldModel->getBody(), 5).getCommand();
				}
				else
					goaliePositioningCommand = doGotoPoint.getCommand();
			}
			else
				goaliePositioningCommand = PreciseGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, actionDist, 
					worldModel->getBody(), 1).getCommand();
		}
    	LOG << "GOALIE PrecideGTP 1 Stamina : " << worldModel->getBody().getStamina() << endl;
	}
	else
    {
		float posIdealAngle, negIdealAngle, deltaPosAngle, 
				deltaNegAngle, maxTurnAngle;

		posIdealAngle = idealBodyAngle;
		negIdealAngle = posIdealAngle - 180;
	   	negIdealAngle = Degree::normalizeAngle(negIdealAngle);

		deltaPosAngle = posIdealAngle - worldModel->getBody().getBodyDir();
		deltaNegAngle = negIdealAngle - worldModel->getBody().getBodyDir();

		deltaPosAngle = Degree::normalizeAngle(deltaPosAngle);
		deltaNegAngle = Degree::normalizeAngle(deltaNegAngle);

		maxTurnAngle = BasicTurn::getTurnAngle(179, worldModel->getBody());

		if (hypot(worldModel->getBody().getPos().getX() - 
			goalieHomePoint.x, worldModel->getBody().getPos().getY() - 
			goalieHomePoint.y) <= actionDist) 
		{
		    if (fabs(deltaPosAngle) - 15 < maxTurnAngle)
				goaliePositioningCommand = TurnAngle(AT_GOALIE_POSITIONING, deltaPosAngle, 
				worldModel->getBody()).getCommand();
	    	else 
	    	{
				if (fabs(deltaNegAngle) - 15 < maxTurnAngle)
		    		goaliePositioningCommand = TurnAngle(AT_GOALIE_POSITIONING, deltaNegAngle, 
					worldModel->getBody()).getCommand();
				else
		    		goaliePositioningCommand = TurnAngle(AT_GOALIE_POSITIONING, deltaNegAngle, 
					worldModel->getBody()).getCommand();
	    	}
		}
		else
		{
	    	if (fabs(deltaPosAngle) - 15 < maxTurnAngle && 
				fabs(deltaPosAngle) > 10)
	    		goaliePositioningCommand = TurnAngle(AT_GOALIE_POSITIONING, deltaNegAngle, 
				worldModel->getBody()).getCommand();
	    	else
 			{
				float distToHome = hypot(worldModel->getBody().getPos().getX() - 
					goalieHomePoint.x, worldModel->getBody().getPos().getY() - 
					goalieHomePoint.y);
				
				float distToGoal = hypot(worldModel->getBody().getPos().getX() - 
					(-52.5), worldModel->getBody().getPos().getY() - 0);
				
				if (distToHome >= 10 || distToGoal > 15)
				{
					if (worldModel->getPlayMode() == PM_FREE_KICK_OPP||
						worldModel->getPlayMode() == PM_OFFSIDE ||
						worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
						worldModel->getPlayMode() == PM_CATCH_FAULT ||
						worldModel->getPlayMode() == PM_BACK_PASS)
					{
						ForbiddenAreaGotoPoint doGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, worldModel->
							getBall().getPos(), 9.15, worldModel->getBody());
						if (doGotoPoint.getStatus() == 0)
						{
							if (doGotoPoint.getCommand())
								delete doGotoPoint.getCommand();
							goaliePositioningCommand = TurnToPoint(AT_GOALIE_POSITIONING, worldModel->getBall().
									getPos(), worldModel->getBody(), 5).getCommand();
						}
						else
							goaliePositioningCommand = doGotoPoint.getCommand();
					}
					else
						goaliePositioningCommand = PreciseGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, actionDist, 
							worldModel->getBody(), 0).getCommand();
				}
				else
				{
					if (worldModel->getPlayMode() == PM_FREE_KICK_OPP||
						worldModel->getPlayMode() == PM_OFFSIDE ||
						worldModel->getPlayMode() == PM_FREE_KICK_FAULT ||
						worldModel->getPlayMode() == PM_CATCH_FAULT ||
						worldModel->getPlayMode() == PM_BACK_PASS)
					{
						ForbiddenAreaGotoPoint doGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, worldModel->
							getBall().getPos(), 9.15, worldModel->getBody());
						if (doGotoPoint.getStatus() == 0)
						{
							if (doGotoPoint.getCommand())
								delete doGotoPoint.getCommand();
							goaliePositioningCommand = TurnToPoint(AT_GOALIE_POSITIONING, worldModel->getBall().
									getPos(), worldModel->getBody(), 5).getCommand();
						}
						else
							goaliePositioningCommand = doGotoPoint.getCommand();
					}
					else
						goaliePositioningCommand = PreciseGotoPoint(AT_GOALIE_POSITIONING, goalieHomePoint, actionDist, 
							worldModel->getBody(), 1).getCommand();
				}
    			LOG << "GOALIE PrecideGTP 1 Stamina : " << worldModel->getBody().getStamina() << endl;
			}
		}
    }
	return AD_NORMAL_VALUE;
}

void GoaliePositioning::execute(Form &form, const Library &library)
{
	command = goaliePositioningCommand;
}