basicdash.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, Mohammad Salehe
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>

#include <Degree.h>
#include <Basics.h>
#include <Defines.h>
#include <BasicTurn.h>
#include <Line.h>
#include <Logger.h>
#include <BasicDash.h>


using namespace std;
using namespace Degree;

//#define FREEKICK_GOTO_POINT_LOG
#define FORBIDDENAREA_GOTO_POINT_LOG

DashDribbleGotoBall::DashDribbleGotoBall(ActionType creator, const Ball &ball, const Body &body): BasicAction(creator)
{
	Body simBody(body);
	Ball simBall(ball);
//	simBody = *body;
//	simBall = *ball;
	float kickableArea, virtualKickableArea;
	float dashPower;
	int dribbleGTBTime = 0;
//	Command dashCommand;
	
	kickableArea = body.getSize() + body.getKickableMargin() + ball.getSize();
	virtualKickableArea = kickableArea * 1.1; // rate 1.1 for caution 
	
	dashPower = 100;
	if (body.getStamina() < max(body.getRecoverDecThr(),
								 body.getEffortDecThr()) *
							 body.getStaminaMax() + 150) // 150 for caution
		dashPower = body.getStaminaIncMax();
	
	//dashCommand.dash(dashPower);	 
	Command *dashCommand = new DashCommand(creator, dashPower);
	
	while (simBall.getBodyVec().getMagnitude() >= virtualKickableArea &&
		   dribbleGTBTime < 30)
	{
		simBody.simulateByAction(dashCommand);
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);	
		dribbleGTBTime++;
	}
	
	if (simBall.getBodyVec().getMagnitude() < virtualKickableArea)
		command = dashCommand;
	else
		command = NULL;
	// dar inja bayad gotoball konad : oon adame ke mikhad in ro seda kone bayad aghel bashe. ;).
}

Command *DashDribbleGotoBall::getCommand()
{
	return command;
}

void ExactGotoPoint::setDashPower(const Body &body, Point targetPoint, 
								float defaultDashPower, float &dashPower)
{
	Body stoppedBody;
	float localDistance;
	
	stoppedBody = body;
	while (stoppedBody.getVel().getMagnitude() > 0.01)
		stoppedBody.simulateByDynamics();
	if (defaultDashPower >= 0)
	{
		localDistance = stoppedBody.getDistance(targetPoint);

#ifdef FREEKICK_GOTO_POINT_LOG
		LOG << "ExactGTP localDistance = " << localDistance << endl;
#endif

		dashPower = ((localDistance * (1 - body.getDecay())) / body.getDashPowerRate())
				/ body.getEffort();

#ifdef FREEKICK_GOTO_POINT_LOG
		LOG << "ExactGTP dashPower = " << dashPower << endl;
#endif

		if (dashPower > body.getMaxPower())
			dashPower = body.getMaxPower();

		if (dashPower > body.getStaminaIncMax() && body.getStamina() < 1350)
	   		dashPower = body.getStaminaIncMax();
	}
	else
	{
		localDistance = stoppedBody.getDistance(targetPoint);
		dashPower = (-localDistance * (1 - body.getDecay()) / body.getDashPowerRate())
				/ body.getEffort();;
		if (dashPower < body.getMinPower())
			dashPower = body.getMinPower();
		if (fabs(dashPower) > (body.getStaminaIncMax() / 2) && 
										body.getStamina() < 1400)
			dashPower = -(body.getStaminaIncMax() / 2);
	}
	
}

ExactGotoPoint::ExactGotoPoint(ActionType creator, Point targetPoint, float deltaDistance, 
		const Body &body, bool negDashFlag): BasicAction(creator)
{
	float pointDistance = hypot(targetPoint.x - body.getPos().getX(), 
								targetPoint.y - body.getPos().getY());
								
	float deltaAngleFront, deltaAngleBack, angleFront, angleBack;
	float cautionAngleBack, cautionAngleFront;
	float dashPower;
	
	command = NULL;
	mode = CT_EMPTY;

	cautionAngleBack = 20;
	cautionAngleFront = 20;
	
	angleFront = body.getBodyDir();
	angleBack = body.getBodyDir() - 180;
	angleBack = Degree::normalizeAngle(angleBack);
	
	if (pointDistance >= deltaDistance)
	{
		Vector pointVector;
		pointVector.setByPoints(body.getPos(), targetPoint);
		
		deltaAngleFront = pointVector.getDirection() - angleFront;
		deltaAngleFront = Degree::normalizeAngle(deltaAngleFront);
		deltaAngleBack = pointVector.getDirection() - angleBack;
		deltaAngleBack = Degree::normalizeAngle(deltaAngleBack);  
		
		if (fabs(deltaAngleFront) < cautionAngleFront)
		{
			setDashPower(body, targetPoint, 100, dashPower);
		   	command = new DashCommand(creator, dashPower);
			mode = CT_DASH;
		}
		else if (fabs(deltaAngleBack) < cautionAngleBack && negDashFlag)
		{
			setDashPower(body, targetPoint, -100, dashPower);
		   	command = new DashCommand(creator, dashPower);
			mode = CT_DASH;
		}
		else
		{
	   		float maxTurnAngle, deltaMaxTurnAngle;
			maxTurnAngle = BasicTurn::getTurnAngle(179, body);
			deltaMaxTurnAngle = fabs(deltaAngleFront) - maxTurnAngle;
	
			if (deltaMaxTurnAngle < cautionAngleFront / 2.00 || !negDashFlag)
			{
				command = TurnAngle(creator, deltaAngleFront, body).getCommand();
				mode = CT_TURN;
			}
			else
			{
				deltaMaxTurnAngle = deltaAngleBack - maxTurnAngle;
		
				if (deltaMaxTurnAngle < cautionAngleBack / 2.00)
				{
					command = TurnAngle(creator, deltaAngleBack, body).getCommand();
					mode = CT_TURN;
				}
				else
				{
					command = TurnAngle(creator, deltaAngleFront, body).getCommand();
					mode = CT_TURN;
				}
			}
		}
	}
}

Command *ExactGotoPoint::getCommand()
{
	return command;
}

CommandType ExactGotoPoint::getMode()
{
	return mode;
}

void PreciseGotoPoint::setDashPower(const Body &body, Point targetPoint, 
								float defaultDashPower, float &dashPower)
{
	Body tempBody;
	tempBody = body;
	
	DashCommand dashCommand(creator, 0);
	
	if (defaultDashPower >= 0)
	{
		
		float tempPower, bestDashPower = 0, bestDistance;
		bestDistance = 1000;
		for (tempPower = 0; tempPower <= 100; tempPower += 5)
		{
			tempBody = body;
	 
			float localDistance;
			dashCommand.setPower(tempPower);
	 
			tempBody.simulateByAction(&dashCommand);
			tempBody.simulateByDynamics();
		
			localDistance = hypot(tempBody.getPos().getX() - targetPoint.x, 
									tempBody.getPos().getY() - targetPoint.y);

			if (localDistance < bestDistance)
			{
				bestDistance = localDistance;
				bestDashPower = tempPower;
			}
		} 
		
		if (hypot(body.getPos().getX() - targetPoint.x,
							body.getPos().getY() - targetPoint.y) > 2)
				bestDashPower = defaultDashPower;			
	
		if (bestDashPower > body.getStaminaIncMax() && body.getStamina() < 1350)
	   		bestDashPower = body.getStaminaIncMax();
	
		dashPower = bestDashPower;
	}
	else
	{
		float tempPower, bestDashPower = 0, bestDistance;

		bestDistance = 1000;

		for (tempPower = 0; tempPower >= -100; tempPower += -5)
		{
	 	   	tempBody = body;

	 	   	float localDistance;
	 	   	dashCommand.setPower(tempPower);

	 	   	tempBody.simulateByAction(&dashCommand);
	 	   	tempBody.simulateByDynamics();
	 	   
	 	   	localDistance = hypot(tempBody.getPos().getX() - targetPoint.x, 
									tempBody.getPos().getY() - targetPoint.y);
			
			if (localDistance < bestDistance)
			{
				bestDistance = localDistance;
				bestDashPower = tempPower;
			}
		} 
		
		if (hypot(body.getPos().getX() - targetPoint.x,
							body.getPos().getY() - targetPoint.y) > 2)
				bestDashPower = defaultDashPower;			


		if (fabs(bestDashPower) > (body.getStaminaIncMax() / 2) && 
										body.getStamina() < 1400)
			bestDashPower = -(body.getStaminaIncMax() / 2);
		
		dashPower = bestDashPower;	
	}
}

PreciseGotoPoint::PreciseGotoPoint(ActionType creator, Point targetPoint, float deltaDistance, 
		const Body &body, bool negDashFlag, float minTurnAngle): BasicAction(creator)
{
	float pointDistance = hypot(targetPoint.x - body.getPos().getX(), 
								targetPoint.y - body.getPos().getY());
								
	float deltaAngleFront, deltaAngleBack, angleFront, angleBack;
	float cautionAngleBack, cautionAngleFront;
	float dashPower;
	
	command = NULL;
	mode = CT_EMPTY;

	cautionAngleBack = minTurnAngle;
	cautionAngleFront = minTurnAngle;
	
	angleFront = body.getBodyDir();
	angleBack = body.getBodyDir() - 180;
	angleBack = Degree::normalizeAngle(angleBack);
	
	if (pointDistance >= deltaDistance)
	{
		Vector pointVector;
		pointVector.setByPoints(body.getPos(), targetPoint);
		
		deltaAngleFront = pointVector.getDirection() - angleFront;
		deltaAngleFront = Degree::normalizeAngle(deltaAngleFront);
		deltaAngleBack = pointVector.getDirection() - angleBack;
		deltaAngleBack = Degree::normalizeAngle(deltaAngleBack);  
		
		if (fabs(deltaAngleFront) < cautionAngleFront)
		{
			setDashPower(body, targetPoint, 100, dashPower);
			LOG << "Meisam2005 : PreciseGTP1 : dashPower : " << dashPower;
		   	command = new DashCommand(creator, dashPower);
			mode = CT_DASH;
		}
		else if (fabs(deltaAngleBack) < cautionAngleBack && negDashFlag)
		{
			setDashPower(body, targetPoint, -100, dashPower);
			LOG << "Meisam2005 : PreciseGTP2 : dashPower : " << dashPower;
		   	command = new DashCommand(creator, dashPower);
			mode = CT_DASH;
		}
		else
		{
	   		float maxTurnAngle, deltaMaxTurnAngle;
			maxTurnAngle = BasicTurn::getTurnAngle(179, body);
			deltaMaxTurnAngle = fabs(deltaAngleFront) - maxTurnAngle;
	
			if (deltaMaxTurnAngle < cautionAngleFront / 2.00 || !negDashFlag)
			{
				command = TurnAngle(creator, deltaAngleFront, body).getCommand();
				mode = CT_TURN;
			}
			else
			{
				deltaMaxTurnAngle = deltaAngleBack - maxTurnAngle;
		
				if (deltaMaxTurnAngle < cautionAngleBack / 2.00)
				{
					command = TurnAngle(creator, deltaAngleBack, body).getCommand();
					mode = CT_TURN;
				}
				else
				{
					command = TurnAngle(creator, deltaAngleFront, body).getCommand();
					mode = CT_TURN;
				}
			}
		}
	}
}

Command *PreciseGotoPoint::getCommand()
{
	return command;
}

CommandType PreciseGotoPoint::getMode()
{
	return mode;
}

DashNormalGotoPoint::DashNormalGotoPoint(ActionType creator, const Point &target, float distance, 
		const Body &body, float maxDashPower): BasicAction(creator)
{
	float deltaAngle, validDeltaAngle;
	Vector targetVector;
	targetVector.setByPoints(body.getPos(), target);
	deltaAngle = getDeltaAngle(targetVector.getDirection(), body.getBodyDir());
	distance = max((float) EPS, distance);
	validDeltaAngle = arcTan2(distance, targetVector.getMagnitude());

	mode = CT_EMPTY;
	command = NULL;
		
	float dashPower = 100;
	if(body.getStamina() < 1300)
		dashPower = body.getStaminaIncMax();	
	if(dashPower > maxDashPower)
		dashPower = maxDashPower;

	if (fabs(deltaAngle) <= 6/*GOTOPOINTMINDELTAANGLE*/ || fabs(deltaAngle) <= validDeltaAngle)
	{
		if (targetVector.getMagnitude() > distance)
		{
			command = new DashCommand(creator, dashPower);
			mode = CT_DASH;