dribble.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 <Pass.h>
#include <Degree.h>
#include <Logger.h>
#include <Basics.h>
#include <Defines.h>
#include <Dribble.h>
#include <BasicKick.h>
#include <BasicTurn.h>


//#define DRIBBLE_LOGLEVEL1


using namespace std;

// class Dribble
Dribble::Dribble(const WorldModel *worldModel): AdvancedAction(worldModel)
{
}

Dribble::~Dribble()
{
}

void Dribble::execute(Form &form, const Library &library)
{
	LOG << "Dribble::execute" << endl;

	command = dribbleCommand;
}

Point Dribble::setByPointInformation(float pointBodyAngle, 
		float cautionDistance)
{
	float dribbleAngle = worldModel->getBody().getBodyDir();
	Vector dribbleVector;
	
	Body bodyNoDash;
	bodyNoDash = worldModel->getBody();
	bodyNoDash.simulateByDynamics();

	float kickableArea = worldModel->getBody().getSize() +
			 worldModel->getBody().getKickableMargin() +
			 worldModel->getBall().getSize();

	
	dribbleAngle += pointBodyAngle;
	dribbleAngle = Degree::normalizeAngle(dribbleAngle);	
	Point dribblePoint2;
	dribbleVector.setAsPolar(kickableArea - cautionDistance, dribbleAngle);

	dribblePoint2.x = bodyNoDash.getPos().getX() + dribbleVector.getX();
	dribblePoint2.y = bodyNoDash.getPos().getY() + dribbleVector.getY();
	return dribblePoint2;
}  

void Dribble::setDribblePoints()
{
	float firstXPathStep = 42;


	dribbleTarget.x = 52.5;

	if (worldModel->getBall().getPos().getX() < firstXPathStep)
		dribbleTarget.y = worldModel->getBall().getPos().getY();
	else
		dribbleTarget.y = 0;	

	if (worldModel->getPlayMode() == PM_PENALTY_READY && 
		worldModel->getVirtualPlayMode() == VPM_PENALTY_TAKEN)
	{
		if (worldModel->getBall().getPos().getX() > 33 && 
			worldModel->getBall().getPos().getX() < 42)
		{
			if (worldModel->getBall().getPos().getY() > 0)
			{
				dribbleTarget.x = 52.5;
				dribbleTarget.y = 34;
			}	
			if (worldModel->getBall().getPos().getY() < 0)
			{
				dribbleTarget.x = 52.5;
				dribbleTarget.y = -34;
			}	
		}
	}
	
	idealDribbleTarget = dribbleTarget;			
	isOpponent = 0;
	opponentInSence = 0;
	targetNums = 36;
	cautionLength = 12;
	//man meisam hastam
	angleWeightRate = 1.6;
	craudWeightRate = 8;
	
	for (int i = 0; i < targetNums; i++)
	{
		targetWeights[i] = 0;
		targetAngles[i] = 0;
	}


	angle[0] = 30;
	angle[1] = -30;
	angle[2] = 70;
	angle[3] = -70;
	angle[4] = 15;
	angle[5] = -15;
	angle[6] = 105;
	angle[7] = -105;
	angle[8] = 135;
	angle[9] = -135;
	angle[10] = 165;
	angle[11] = -165;

	virtualWeight[0] = 6;
	virtualWeight[1] = 6;
	virtualWeight[2] = 5;
	virtualWeight[3] = 5;
	virtualWeight[4] = 4;
	virtualWeight[5] = 4;
	virtualWeight[6] = 3;
	virtualWeight[7] = 3;
	virtualWeight[8] = 2;
	virtualWeight[9] = 2;
	virtualWeight[10] = 1;
	virtualWeight[11] = 1;
	
	noise1[0] = .1;
	noise1[1] = .1;
	noise1[2] = .3;
	noise1[3] = .3;
	noise1[4] = .1;
	noise1[5] = .1;
	noise1[6] = .3;
	noise1[7] = .3;
	noise1[8] = .2;
	noise1[9] = .2;
	noise1[10] = .2;
	noise1[11] = .2;


	noise2[0] = .1;
	noise2[1] = .1;
	noise2[2] = .4;
	noise2[3] = .4;
	noise2[4] = .3;
	noise2[5] = .3;
	noise2[6] = .4;
	noise2[7] = .4;
	noise2[8] = .3;
	noise2[9] = .3;
	noise2[10] = .5;
	noise2[11] = .5;
	if (worldModel->getBall().getVel().getMagnitude() > 1.2)
	{
		noise1[0] = .3;
		noise1[1] = .3;
		noise1[2] = .5;
		noise1[3] = .5;
		noise1[4] = .3;
		noise1[5] = .3;
		noise1[6] = .5;
		noise1[7] = .5;
		noise1[8] = .4;
		noise1[9] = .4;
		noise1[10] = .4;
		noise1[11] = .4;
	
	
		noise2[0] = .5;
		noise2[1] = .5;
		noise2[2] = .5;
		noise2[3] = .5;
		noise2[4] = .5;
		noise2[5] = .5;
		noise2[6] = .5;
		noise2[7] = .5;
		noise2[8] = .5;
		noise2[9] = .5;
		noise2[10] = .5;
		noise2[11] = .5;
	}


	dashPower[0] = 100;
	dashPower[1] = 100;
	dashPower[2] = 80;
	dashPower[3] = 80;
	dashPower[4] = 100;
	dashPower[5] = 100;
	dashPower[6] = 60;
	dashPower[7] = 60;
	dashPower[8] = 60;
	dashPower[9] = 60;
	dashPower[10] = 0;
	dashPower[11] = 0;



	maxDist[0] = 1.6;
	maxDist[1] = 1.6;
	maxDist[2] = .3;
	maxDist[3] = .3;
	maxDist[4] = .7;
	maxDist[5] = .7;
	maxDist[6] = .4;
	maxDist[7] = .4;
	maxDist[8] = .4;
	maxDist[9] = .4;
	maxDist[10] = .4;
	maxDist[11] = .4;


	cycleNums[0] = 1;
	cycleNums[1] = 1;
	cycleNums[2] = 1;
	cycleNums[3] = 1;
	cycleNums[4] = 1;
	cycleNums[5] = 1;
	cycleNums[6] = 1;
	cycleNums[7] = 1;
	cycleNums[8] = 1;
	cycleNums[9] = 1;
	cycleNums[10] = 1;
	cycleNums[11] = 1;



	kickableNoise[0] = .1;
	kickableNoise[1] = .1;
	kickableNoise[2] = .2;
	kickableNoise[3] = .2;
	kickableNoise[4] = .1;
	kickableNoise[5] = .1;
	kickableNoise[6] = .2;
	kickableNoise[7] = .2;
	kickableNoise[8] = .1;
	kickableNoise[9] = .1;
	kickableNoise[10] = .1;
	kickableNoise[11] = .1;


	angleNoise[0] = 180;
	angleNoise[1] = 180;
	angleNoise[2] = 70;
	angleNoise[3] = 70;
	angleNoise[4] = 180;
	angleNoise[5] = 70;
	angleNoise[6] = 70;
	angleNoise[7] = 180;
	angleNoise[8] = 180;
	angleNoise[9] = 180;
	angleNoise[10] = 180;
	angleNoise[11] = 180;
	

	kickPower[0] = .58;
	kickPower[1] = .58;
	kickPower[2] = .9;
	kickPower[3] = .9;
	kickPower[4] = .9;
	kickPower[5] = .9;
	kickPower[6] = .9;
	kickPower[7] = .9;
	kickPower[8] = .9;
	kickPower[9] = .9;
	kickPower[10] = .9;
	kickPower[11] = .9;

	
	for (int i = 0; i < 12; i++)
	{
		dribblePoints[i] = setByPointInformation(angle[i], noise1[i]); 
		virtualDribblePoints[i] = setByPointInformation(angle[i], noise1[i]); 
	}
	
	for (int i = 0; i < 12; i++)
		dribbleWeights[i] = 0;
}

void Dribble::setDribbleArgumentsByOpponent()
{

	
	float firstXPathStep = 42;


	dribbleTarget.x = 52.5;

	if (worldModel->getBall().getPos().getX() < firstXPathStep)
		dribbleTarget.y = worldModel->getBall().getPos().getY();
	else
		dribbleTarget.y = 0;	


	if (worldModel->getPlayMode() == PM_PENALTY_READY && 
		worldModel->getVirtualPlayMode() == VPM_PENALTY_TAKEN)
	{
		if (worldModel->getBall().getPos().getX() > 33 && 
			worldModel->getBall().getPos().getX() < 42)
		{
			if (worldModel->getBall().getPos().getY() > 0)
			{
				dribbleTarget.x = 52.5;
				dribbleTarget.y = 34;
			}	
			if (worldModel->getBall().getPos().getY() < 0)
			{
				dribbleTarget.x = 52.5;
				dribbleTarget.y = -34;
			}	
		}
	}
	
	idealDribbleTarget = dribbleTarget;	
	isOpponent = 0;
	targetNums = 36;
	cautionLength = 12;
	angleWeightRate = 1.6;
	craudWeightRate = 8;
	
	for (int i = 0; i < targetNums; i++)
	{
		targetWeights[i] = 0;
		targetAngles[i] = 0;
	}
	
	angle[0] = 30;
	angle[1] = -30;
	angle[2] = 70;
	angle[3] = -70;
	angle[4] = 15;
	angle[5] = -15;
	angle[6] = 105;
	angle[7] = -105;
	angle[8] = 135;
	angle[9] = -135;
	angle[10] = 165;
	angle[11] = -165;

	virtualWeight[0] = 6;
	virtualWeight[1] = 6;
	virtualWeight[2] = 5;
	virtualWeight[3] = 5;
	virtualWeight[4] = 4;
	virtualWeight[5] = 4;
	virtualWeight[6] = 3;
	virtualWeight[7] = 3;
	virtualWeight[8] = 2;
	virtualWeight[9] = 2;
	virtualWeight[10] = 1;
	virtualWeight[11] = 1;
	
	noise1[0] = .1;
	noise1[1] = .1;
	noise1[2] = .2;
	noise1[3] = .2;
	noise1[4] = .1;
	noise1[5] = .1;
	noise1[6] = .2;
	noise1[7] = .2;
	noise1[8] = .1;
	noise1[9] = .1;
	noise1[10] = .1;
	noise1[11] = .1;


	noise2[0] = .3;
	noise2[1] = .3;
	noise2[2] = .4;
	noise2[3] = .4;
	noise2[4] = .3;
	noise2[5] = .3;
	noise2[6] = .4;
	noise2[7] = .4;
	noise2[8] = .3;
	noise2[9] = .3;
	noise2[10] = .4;
	noise2[11] = .4;


	dashPower[0] = 100;
	dashPower[1] = 100;
	dashPower[2] = 80;
	dashPower[3] = 80;
	dashPower[4] = 100;
	dashPower[5] = 100;
	dashPower[6] = 60;
	dashPower[7] = 60;
	dashPower[8] = 60;
	dashPower[9] = 60;
	dashPower[10] = 50;
	dashPower[11] = 50;

	maxDist[0] = 0.4;
	maxDist[1] = 0.4;
	maxDist[2] = 0;
	maxDist[3] = 0;
	maxDist[4] = 0;
	maxDist[5] = 0;
	maxDist[6] = 0;
	maxDist[7] = 0;
	maxDist[8] = 0;
	maxDist[9] = 0;
	maxDist[10] = 0;
	maxDist[11] = 0;

	cycleNums[0] = 1;
	cycleNums[1] = 1;
	cycleNums[2] = 1;
	cycleNums[3] = 1;
	cycleNums[4] = 1;
	cycleNums[5] = 1;
	cycleNums[6] = 1;
	cycleNums[7] = 1;
	cycleNums[8] = 1;
	cycleNums[9] = 1;
	cycleNums[10] = 1;
	cycleNums[11] = 1;

	kickableNoise[0] = .1;
	kickableNoise[1] = .1;
	kickableNoise[2] = .2;
	kickableNoise[3] = .2;
	kickableNoise[4] = .1;
	kickableNoise[5] = .1;
	kickableNoise[6] = .2;
	kickableNoise[7] = .2;
	kickableNoise[8] = .1;
	kickableNoise[9] = .1;
	kickableNoise[10] = .1;
	kickableNoise[11] = .1;

	angleNoise[0] = 180;
	angleNoise[1] = 180;
	angleNoise[2] = 70;
	angleNoise[3] = 70;
	angleNoise[4] = 180;
	angleNoise[5] = 70;
	angleNoise[6] = 70;
	angleNoise[7] = 180;
	angleNoise[8] = 180;
	angleNoise[9] = 180;
	angleNoise[10] = 180;
	angleNoise[11] = 180;

	kickPower[0] = 1;
	kickPower[1] = 1;
	kickPower[2] = 1;
	kickPower[3] = 1;
	kickPower[4] = 1;
	kickPower[5] = 1;
	kickPower[6] = 1;
	kickPower[7] = 1;
	kickPower[8] = 1;
	kickPower[9] = 1;
	kickPower[10] = 1;
	kickPower[11] = 1;
	
	
	for (int i = 0; i < 12; i++)
	{
		dribblePoints[i] = setByPointInformation(angle[i], noise1[i]); 
		virtualDribblePoints[i] = setByPointInformation(angle[i], noise1[i]); 
	}
	
	for (int i = 0; i < 12; i++)
		dribbleWeights[i] = 0;
}

void Dribble::setOpponentInWeight(float senceArea)
{
	opponentInSence = 0;
	Point playerPoint;
	for (int i = 0; i < 12; i++)
	{
		for (int j = 0; j < FULL_PLAYERS_NUM; j++)
		{
	/*BY AHMAD*/    if (worldModel->getFullPlayer(TID_TEAMMATE, j).isValid() &&
				!worldModel->getFullPlayer(TID_TEAMMATE, j).isBody())
			{
				playerPoint = worldModel->getFullPlayer(TID_TEAMMATE, j).getPos();
				
				float playerDist =  hypot(dribblePoints[i].x - playerPoint.x, 
						dribblePoints[i].y - playerPoint.y);
	
				if (playerDist < senceArea)
				{
					dribbleWeights[i] -= senceArea - playerDist;	
					opponentInSence = 1;
 				}
			}
	
			if (worldModel->getFullPlayer(TID_OPPONENT, j).isValid())
			{
				playerPoint = worldModel->getFullPlayer(TID_OPPONENT, j).getPos();
				
				float playerDist =  hypot(dribblePoints[i].x - playerPoint.x, 
						dribblePoints[i].y - playerPoint.y);
	
				if (playerDist < senceArea)
				{
					dribbleWeights[i] -= senceArea - playerDist;	
					opponentInSence = 1;
 				}
			}	
		}
		for (int j = 0; j < HALF_PLAYERS_NUM; j++)
		{
	/*BY AHMAD*/   if (worldModel->getHalfPlayer(TID_TEAMMATE, j).isValid())
			{
				playerPoint = worldModel->getHalfPlayer(TID_TEAMMATE, j).getPos();
				
				float playerDist =  hypot(dribblePoints[i].x - playerPoint.x, 
						dribblePoints[i].y - playerPoint.y);
	
				if (playerDist < senceArea)
				{
					dribbleWeights[i] -= senceArea - playerDist;	
					opponentInSence = 1;
 				}
			}

			if (worldModel->getHalfPlayer(TID_OPPONENT, j).isValid())
			{
				playerPoint = worldModel->getHalfPlayer(TID_OPPONENT, j).getPos();

				float playerDist =  hypot(dribblePoints[i].x - playerPoint.x, 
						dribblePoints[i].y - playerPoint.y);

				if (playerDist < senceArea)
				{
					dribbleWeights[i] -= senceArea - playerDist;	
					opponentInSence = 1;
 				}
			}	
		}

		for (int j = 0; j < QUARTER_PLAYERS_NUM; j++)