/* *  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 InterceptPlayerInterceptPlayer::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 functionsbool 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 functionsvoid 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 InterceptCalculatevoid 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()