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

#include <cmath>
#include <cassert>

#include <Logger.h>
#include <Degree.h>
#include <Defines.h>
#include <WorldModel.h>
#include <InterceptCalculate.h>


using namespace std;
using namespace Degree;

// class InterceptPlayer

InterceptPlayer::InterceptPlayer()
{
	player = NULL;
	withTurnVel = NULL;
	withoutTurnVel = NULL;
	withTurnDashDist = NULL;
	withoutTurnDashDist = NULL;
}

void InterceptPlayer::simulate()
{
	withTurnVel++;
	withTurnDashDist++;

	withoutTurnVel++;
	withoutTurnDashDist++;
}

void InterceptPlayer::getReadyToRun()
{
	// Reseting dynamic variables
	setCatchBall(false);

	withTurnVel = withTurnVelArchive;
	withTurnDashDist = withTurnDashDistArchive;

	withoutTurnVel = withoutTurnVelArchive;
	withoutTurnDashDist = withoutTurnDashDistArchive;
}

void InterceptPlayer::init(const Player &playerArg, const Ball &ball,
		bool kickingFlagArg, unsigned kickCycles,
		unsigned seeBallDelayArg, float slowDashPower,
		float fastDashPower, float directDashPower,
		float posDevDashDist, bool blockedPlayer, bool checkForPenaltyBox)
{
	unsigned cyclesCounter;

	blockedFlag = blockedPlayer;

	player = &playerArg;
	simPlayer = *player;

//	Body tempBody; // A Theory says these lines are bug.
//	simPlayer.simulateByDynamics(tempBody);

    if (player->isGoalie())
	{
		if (!checkForPenaltyBox ||
			(player->getTeamId() == TID_TEAMMATE &&
			 player->getPos().getX() < -36.4 &&
			 fabs(player->getPos().getY()) < 19.6) ||
			(player->getTeamId() == TID_OPPONENT &&
			 player->getPos().getX() > 36.4 &&
			 fabs(player->getPos().getY()) < 19.6))
			kickableRadius = player->getCatchableAreaL();
		else
			kickableRadius = player->getSize() +
					player->getKickableMargin() +
					ball.getSize();
	}
	else
		kickableRadius = player->getSize() +
				player->getKickableMargin() +
				ball.getSize();

	slowAccel = slowDashPower * player->getDashPowerRate() *
				player->getEffortMax();
	fastAccel = fastDashPower * player->getDashPowerRate() *
				player->getEffortMax();
	directAccel = directDashPower * player->getDashPowerRate() *
				player->getEffortMax();

	seeBallDelay = seeBallDelayArg;
	setKicking(kickingFlagArg);

	// Setting start values
	withTurnVelArchive[0] = 0;
	withTurnDashDistArchive[0] = posDevDashDist;

	withoutTurnVelArchive[0] = player->getVel().getMagnitude();
	withoutTurnDashDistArchive[0] = posDevDashDist;

	if (isLogging())
		LOG << "NewP: " << player->getTeamId() << " " << player->getUniNum()
			<< " Pos" << simPlayer.getPos() << " ----------" << endl
			<< "WithTurnDashVel[0]: " << withTurnDashDistArchive[0] << " "
			<< withTurnVelArchive[0] << endl
			<< "NoTurnDashVel[0]: " << withoutTurnDashDistArchive[0] << " "
			<< withoutTurnVelArchive[0] << endl;

	// Setting other values
	for (cyclesCounter = 1; cyclesCounter < INCA_MAX_INTERCEPT_TIME; cyclesCounter++)
	{
		// Copying values from last cycle
		withTurnDashDistArchive[cyclesCounter] =
				withTurnDashDistArchive[cyclesCounter - 1];
		withoutTurnDashDistArchive[cyclesCounter] =
				withoutTurnDashDistArchive[cyclesCounter - 1];

		withTurnVelArchive[cyclesCounter] =
				withTurnVelArchive[cyclesCounter - 1];
		withoutTurnVelArchive[cyclesCounter] =
				withoutTurnVelArchive[cyclesCounter - 1];

		// Accelerating
		if (!isKicking() &&
			cyclesCounter <= seeBallDelay + kickCycles) // The ball is kicking or I still don't know it has kicked.
		{
			withTurnVelArchive[cyclesCounter] += slowAccel;
			withoutTurnVelArchive[cyclesCounter] += directAccel;

			if (withTurnVelArchive[cyclesCounter] > player->getSpeedMax())
				withTurnVelArchive[cyclesCounter] = player->getSpeedMax();
			if (withoutTurnVelArchive[cyclesCounter] > player->getSpeedMax())
				withoutTurnVelArchive[cyclesCounter] = player->getSpeedMax();
		}
		else
		{
			if (cyclesCounter > seeBallDelay + kickCycles)
			{
				withoutTurnVelArchive[cyclesCounter] += fastAccel;
				if (withoutTurnVelArchive[cyclesCounter] > player->getSpeedMax())
					withoutTurnVelArchive[cyclesCounter] = player->getSpeedMax();
			}

			if (cyclesCounter > seeBallDelay + kickCycles + 1)
			{
				withTurnVelArchive[cyclesCounter] += fastAccel;
				if (withTurnVelArchive[cyclesCounter] > player->getSpeedMax())
					withTurnVelArchive[cyclesCounter] = player->getSpeedMax();
			}
		}

		// Moving
		withTurnDashDistArchive[cyclesCounter] += 
				withTurnVelArchive[cyclesCounter];
		withoutTurnDashDistArchive[cyclesCounter] += 
				withoutTurnVelArchive[cyclesCounter];

		// Simulating
		withTurnVelArchive[cyclesCounter] *= player->getDecay();
		withoutTurnVelArchive[cyclesCounter] *= player->getDecay();
	}
}

void InterceptPlayer::finalizeIntercept(const Ball &ball, unsigned interceptTimeArg)
{
	setCatchBall(true);

	interceptTime = interceptTimeArg;
	interceptPoint = ball.getPos();
}

bool InterceptPlayer::isBallKickable(const Ball &ball)
{
	if (player->getDistance(ball) <= kickableRadius)
		return true;
	return false;
}

bool InterceptPlayer::checkCatchBall(const Ball &ball)
{
	bool flag = false;
	float ballDist;
	float deltaAngle;
	float validDeltaAngle;
	Vector vector;

	if (isLogging())
		LOG << "Player: " << player->getTeamId() << " " << player->getUniNum()
			<< " Pos" << simPlayer.getPos() << " ----------" << endl
			<< "WithTurnDashVel: " << *withTurnDashDist << " "
			<< *withTurnVel << endl
			<< "NoTurnDashVel: " << *withoutTurnDashDist << " "
			<< *withoutTurnVel << endl;

	// Without turn catch ball check
	ballDist = simPlayer.getDistance(ball) - *withoutTurnDashDist;

	if (isLogging())
		LOG << "NoTurnBallDist: " << ballDist << endl;

	if (ballDist < kickableRadius)
	{
		if (player->getBodyDir() != NOVALUE)
		{
			vector.setByPoints(simPlayer.getPos(), ball.getPos());
			validDeltaAngle = arcTan(kickableRadius / vector.getMagnitude());

			deltaAngle = getDeltaAngle(vector.getDirection(),
					simPlayer.getBodyDir());

			if (fabs(deltaAngle) <= validDeltaAngle ||
				fabs(deltaAngle) <= INCA_MIN_DELTA_ANGLE)
				flag = true;
			else // Checking back dash.
			{
				deltaAngle = getDeltaAngle(vector.getDirection(),
						simPlayer.getBodyDir() + 180);

				if (fabs(deltaAngle) <= validDeltaAngle ||
					fabs(deltaAngle) <= INCA_MIN_DELTA_ANGLE)
					flag = true;
			}
		}

		// With turn catch ball check
		if (flag == false)
		{
			ballDist = simPlayer.getDistance(ball) - *withTurnDashDist;

			if (isLogging())
				LOG << "WithTurnBallDist: " << ballDist << endl;

			if (ballDist < kickableRadius)
				flag = true;
		}
	}

	if (flag)
	{

		if (isLogging())
			LOG << "*** I catched the ball ***" << endl;

		return true;
	}

	return false;
}

void InterceptPlayer::checkForExtraCatch(const Ball &ball)
{
	float ballDist = simPlayer.getDistance(ball) - *withTurnDashDist;

	if (isLogging())
		LOG << "Player: " << player->getTeamId() << " " << player->getUniNum()
			<< " Pos" << simPlayer.getPos() << " ----EC----" << endl
			<< "WithTurnDashVel: " << *withTurnDashDist << " "
			<< *withTurnVel << endl
			<< "WithTurnBallDist: " << ballDist << endl;

	if (ballDist < kickableRadius)
	{
		extraCatchedFlag = true;

		if (isLogging())
			LOG << "*** I extra catched the ball ***" << endl;
	}
}

// Getting functions
bool InterceptPlayer::isLogging() const
{
	return loggingFlag;
}

bool InterceptPlayer::isKicking() const
{
	return kickingFlag;
}

bool InterceptPlayer::isBlocked() const
{
	return blockedFlag;
}

bool InterceptPlayer::isCatchBall() const
{
	return catchBallFlag;
}

bool InterceptPlayer::isExtraCatched() const
{
	return extraCatchedFlag;
}

float InterceptPlayer::getDashDist() const
{
	return *withTurnDashDist;
}

unsigned InterceptPlayer::getInterTime() const
{
	return interceptTime;
}

Point InterceptPlayer::getInterPoint() const
{
	return interceptPoint;
}

const Player &InterceptPlayer::getPlayer() const
{
	return *player;
}

float InterceptPlayer::getKickableRadius() const
{
	return kickableRadius;
}

// Setting functions
void InterceptPlayer::setLogging(bool loggingFlagArg)
{
	loggingFlag = loggingFlagArg;
}

void InterceptPlayer::setKicking(bool kickingFlagArg)
{
	kickingFlag = kickingFlagArg;
}

void InterceptPlayer::setCatchBall(bool catchBallFlagArg)
{
	catchBallFlag = catchBallFlagArg;
}

void InterceptPlayer::setExtraCatched(bool extraCatchedFlagArg)
{
	extraCatchedFlag = extraCatchedFlagArg;
}

void InterceptPlayer::setKickableRadius(float kickableRadiusArg)
{
	kickableRadius = kickableRadiusArg;
}


// class InterceptCalculate

void InterceptCalculate::run()
{
	unsigned finishTimer = NOVALUE;
	unsigned interceptsCycle = 0;
	unsigned kickCyclesCounter = kickCycles;
	unsigned i;

	if (isLogging())
		LOG << "Running Intercepts." << endl;

	getReadyToRun();

	// checking the moment
	if (kickCyclesCounter == 0)
		for (i = 0; i < playersNum; i++)
			if (interceptPlayers[i].isBallKickable(virtualBall))
			{
				interceptPlayers[i].finalizeIntercept(virtualBall,
						interceptsCycle);

				if (!interceptPlayers[i].isBlocked())
				{
					checkedPlayers[checkedPlayersNum] = i;
					checkedPlayersNum++;
					if (finishTimer == NOVALUE)
						finishTimer = 0;

					if (checkedPlayersNum == MAX_CHECK_PLAYERS ||
						interceptPlayers[i].getPlayer().isGoalie())
					{
						finalizeIntercepts();
						return;
					}
				}
				else
					setBlockedBall();
			}

	// Running GoToBalls
	while (1)
	{
		interceptsCycle++;
		if (finishTimer != NOVALUE)
			finishTimer++;

		if (isLogging())
			LOG << "Time: " << interceptsCycle << " - "
				<< "FinishTime: " << finishTimer << " ---------------------"
				<< endl	<< "Simulating objects..." << endl;

		// simulating the ball and players
		for (i = 0; i < playersNum; i++)
			if (!interceptPlayers[i].isCatchBall())
				interceptPlayers[i].simulate();

		if (kickCyclesCounter > 0)
			kickCyclesCounter--;
		if (kickCyclesCounter == 0)
			simulateVirtualBall();

		if (isLogging())
			LOG << "Ball:"
				<< " Pos" << virtualBall.getPos()
				<< " Vel" << virtualBall.getVel() << endl
				<< "Checking for catches..." << endl;

		// Checking players for catching the ball
		for (i = 0; i < playersNum; i++)
 			if (!interceptPlayers[i].isCatchBall())
			{
				if (interceptPlayers[i].checkCatchBall(virtualBall))
				{
					interceptPlayers[i].finalizeIntercept(virtualBall,
						interceptsCycle);

					if (!interceptPlayers[i].isBlocked())
					{
						checkedPlayers[checkedPlayersNum] = i;
						checkedPlayersNum++;
						if (finishTimer == NOVALUE)
							finishTimer = 0;

						if (checkedPlayersNum == MAX_CHECK_PLAYERS ||
							interceptPlayers[i].getPlayer().isGoalie())
						{
							finalizeIntercepts();
							return;
						}
					}
					else
						setBlockedBall();
				}

			}
			else if (!interceptPlayers[i].isExtraCatched())
				interceptPlayers[i].checkForExtraCatch(virtualBall);

		if (finishTimer != NOVALUE &&
			finishTimer >= shareCycles)
		{
			if (isLogging())
				LOG << "*** Finish : finishTimer overflow ***" << endl;

			finalizeIntercepts();
			return;
		}

		if (interceptsCycle > INCA_MAX_INTERCEPT_TIME - 5)
		{
			if (isLogging())
				LOG << "*** Finish : interceptCycles overflow ***" << endl;

			finalizeIntercepts();
			return;
		}

		if (playersNum == 0 &&
			virtualBall.getVel().getMagnitude() < 0.05) // the ball is stoped in an empty field.
		{
			if (isLogging())
				LOG << "*** Finish : Ownerless ball in the field ***" << endl;

			finalizeIntercepts();
			return;
		}

		if (WorldModel::getOutDistance(virtualBall.getPos()) > INCA_OUT_DIST)
		{
			if (isLogging())
				LOG << "*** Finish : The ball goes out ***" << endl;

			playMode = PM_KICK_OFF_OPP;
			finalizeIntercepts();
			return;
		}
	}
}

void InterceptCalculate::prepareSum()
{
	if (checkedPlayersNum == 0)
		return;

	fastestCheckedPlayerNum = 0;
	fastestPlayerNum = checkedPlayers[0];

	for (unsigned i = 0; i < checkedPlayersNum; i++)
		if (getCheckedInterPlayer(i).getPlayer().getTeamId() == TID_TEAMMATE)
		{
			fastestTmmCheckedPlayerNum = i;
			fastestTmmPlayerNum = checkedPlayers[i];
			break;
		}

	for (unsigned i = 0; i < checkedPlayersNum; i++)
		if (getCheckedInterPlayer(i).getPlayer().getTeamId() == TID_OPPONENT)
		{
			fastestOppCheckedPlayerNum = i;
			fastestOppPlayerNum = checkedPlayers[i];
			break;
		}
}

void InterceptCalculate::initTeamsProps()