positioning.cpp

mersad源码 03年robocup 季军 可以研究一下大家

/*
 *  Copyright 2002-2004, Mersad Team, Allame 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.
 *
 *  Created by: Meisam Vosoughpour
 *  Released on Friday 1 April 2005 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>

#include <Logger.h>
#include <BasicDash.h>
#include <BasicTurn.h>
#include <Positioning.h>
#include <Basics.h>


using namespace std;

Positioning::Positioning(const WorldModel *worldModel):
		worldModel(worldModel)
{
	setSimplePos();
}

void Positioning::setSimplePos()
{
	attraction[0] = .35;
	ballPos[0] = Point(-52.5, 0);
	playerPos[0][4] = Point(-49, 9);
	playerPos[0][2] = Point(-49, -3);
	playerPos[0][3] = Point(-49, 3);
	playerPos[0][1] = Point(-49, -9);
	playerPos[0][5] = Point(-34, 17);
	playerPos[0][7] = Point(-45, 0);
	playerPos[0][6] = Point(-34, -17);
	playerPos[0][8] = Point(-13, 0);
	playerPos[0][9] = Point(-10, -20);
	playerPos[0][10] = Point(-10, 20);



	attraction[1] = .35;
	ballPos[1] = Point(-23, 0);
	playerPos[1][4] = Point(-30, 15);
	playerPos[1][2] = Point(-30, -5);
	playerPos[1][3] = Point(-30, 5);
	playerPos[1][1] = Point(-30, -15);
	playerPos[1][5] = Point(-20, 12);
	playerPos[1][7] = Point(-30, 0);
	playerPos[1][6] = Point(-20, -12);
	playerPos[1][8] = Point(0, 0);
	playerPos[1][9] = Point(0, -18);
	playerPos[1][10] = Point(0, 18);

	attraction[3] = .35;
	ballPos[3] = Point(17, 0);
	playerPos[3][4] = Point(14, 17);
	playerPos[3][2] = Point(14, -5.8);
	playerPos[3][3] = Point(14, 5.8);
	playerPos[3][1] = Point(14, -17);
	playerPos[3][5] = Point(13, 20);
	playerPos[3][7] = Point(15, 0);
	playerPos[3][6] = Point(13, -20);
	playerPos[3][8] = Point(21.5, 0);
	playerPos[3][9] = Point(23, -10);
	playerPos[3][10] = Point(23, 10);


	attraction[2] = .35;
	ballPos[2] = Point(0, 0);

	for (int i = 1; i < 11; i++)
	{	
	
		playerPos[2][i].x = playerPos[1][i].x + (((ballPos[2].x - ballPos[1].x) / 
				(ballPos[3].x - ballPos[1].x)) * 
				(playerPos[3][i].x - playerPos[1][i].x));

		playerPos[2][i].y = playerPos[1][i].y + (((ballPos[2].x - ballPos[1].x) / 
				(ballPos[3].x - ballPos[1].x)) * 
				(playerPos[3][i].y - playerPos[1][i].y));
		if (i == 5)
		{
			playerPos[2][i].y = 6; 
			playerPos[2][i].x = .5;
		}
		if (i == 6)
		{
			playerPos[2][i].y = -6; 
			playerPos[2][i].x = .5;
		}	
	}



	attraction[7] = .35;
	ballPos[7] = Point(52.5, 0);
	playerPos[7][4] = Point(10, 20);
	playerPos[7][2] = Point(0, -10);
	playerPos[7][3] = Point(0, 10);
	playerPos[7][1] = Point(10, -20);
	playerPos[7][5] = Point(46, 37);
	playerPos[7][7] = Point(14, 0);
	playerPos[7][6] = Point(46, -37);
	playerPos[7][8] = Point(46.5, 0);
	playerPos[7][9] = Point(48, -(+3));
	playerPos[7][10] = Point(48, -(-3));

	attraction[4] = .35;
	ballPos[4] = Point(28, 0);

	for (int i = 1; i < 11; i++)
	{	
		playerPos[4][i].x = playerPos[3][i].x + (((ballPos[4].x - ballPos[3].x) / 
				(ballPos[7].x - ballPos[3].x)) * 
				(playerPos[7][i].x - playerPos[3][i].x));

		playerPos[4][i].y = playerPos[3][i].y + (((ballPos[4].x - ballPos[3].x) / 
				(ballPos[7].x - ballPos[3].x)) * 
				(playerPos[7][i].y - playerPos[3][i].y));
	}

	attraction[5] = .4;
	ballPos[5] = Point(52.5, 0);
	playerPos[5][4] = Point(10, 20);
	playerPos[5][2] = Point(0, -10);
	playerPos[5][3] = Point(0, 10);
	playerPos[5][1] = Point(10, -20);
	playerPos[5][5] = Point(32, 18);
	playerPos[5][7] = Point(14, 0);
	playerPos[5][6] = Point(32, -18);
	playerPos[5][8] = Point(49.5, 0);
	playerPos[5][9] = Point(50, -6);
	playerPos[5][10] = Point(50, 6);
}

void Positioning::setHomePoints()
{
	int region;
	for (region = 0; region < 6; region++) 
	{		
		if (worldModel->getBall().getPos().getX() < ballPos[0].x)
		{
			region = 0;
			break;
		}
		if (worldModel->getBall().getPos().getX() > ballPos[4].x)
		{
			region = 4;
			break;
		}
		if (worldModel->getBall().getPos().getX() < ballPos[region + 1].x)
			break;
	}

	float attract = .3;

	for (int i = 1; i < 11; i++)
	{
		basicPos[i].x = playerPos[region][i].x + (((worldModel->getBall().getPos().getX() - 
			ballPos[region].x) / (ballPos[region + 1].x - ballPos[region].x)) * 
			(playerPos[region + 1][i].x - playerPos[region][i].x));				
				
		basicPos[i].y = playerPos[region][i].y + (((worldModel->getBall().getPos().getX() - 
			ballPos[region].x) / (ballPos[region + 1].x - ballPos[region].x)) * 
			(playerPos[region + 1][i].y - playerPos[region][i].y));				
		
		attract = attraction[region] + (((worldModel->getBall().getPos().getX() - 
			ballPos[region].x) / (ballPos[region + 1].x - ballPos[region].x)) * 
			(attraction[region + 1] - attraction[region]));

//		if ((i == 3 || i == 2 || (i == 1 && worldModel->getBall().getPos().getY() > 0)
//		 		|| (i == 4 && worldModel->getBall().getPos().getY() < 0)
//				|| i == 7) && 
//				worldModel->getBall().getPos().getX() < 60)
//			attract = .05;	


/////////////////PLAYER 9&10&11 BEG  BUUUUUUUUUUUUUGGGG LFOKJGJ
//	HHHHHHHHHHHHEEEEEEEEEEEEEEEEEEEELLLLLLLLLLLLLLLLLLLLPPPPPPPPPPPPPPP
		if (i == 9 && worldModel->getBall().getPos().getX() > 40
				&& fabs(worldModel->getBall().getPos().getY()) < 15)
			attract = .1;	
		if (i == 10 && worldModel->getBall().getPos().getX() > 40
				&& fabs(worldModel->getBall().getPos().getY()) < 15)
			attract = .1;	
		if (i == 11 && worldModel->getBall().getPos().getX() > 40
				&& fabs(worldModel->getBall().getPos().getY()) < 15)
			attract = .1;	
/////////////////PLAYER 9&10&11 END


		
		if (i == 8 && basicPos[i].x > 44.5)
			basicPos[i].x = 44.5; 

///////////////////////OFFENSE SECTION
		

		if (i == 5 && worldModel->getBall().getPos().getY() < -5 &&
			worldModel->getBall().getPos().getX() > 35)
		{	
			attract = 2;	
			basicPos[i].x -= 0;
			if (basicPos[i].x > 39)
				basicPos[i].x = 39;
		}

		if (i == 5 && worldModel->getBall().getPos().getY() < -5 &&
			worldModel->getBall().getPos().getX() > 45)
		{	
			attract = 2;	
			basicPos[i].x -= 0;
			if (basicPos[i].x > 40)
				basicPos[i].x = 40;
		}
		
		if (i == 6 && worldModel->getBall().getPos().getY() > 5 &&
			worldModel->getBall().getPos().getX() > 35)
		{	
			attract = 2;	
			basicPos[i].x -= 0;
			if (basicPos[i].x > 39)
				basicPos[i].x = 39;
		}

		if (i == 6 && worldModel->getBall().getPos().getY() > 5 &&
			worldModel->getBall().getPos().getX() > 45)
		{	
			attract = 2;	
			basicPos[i].x -= 0;
			if (basicPos[i].x > 40)
				basicPos[i].x = 40;
		}
			
///////////////////////OFFENSE SECTION

		basicPos[i].y += worldModel->getBall().getPos().getY() * attract;
		
		if ((i == 2 || i == 3) && basicPos[i].x > -1)
			basicPos[i].x = -1;
		
		if ((i == 1 || i == 4) && basicPos[i].x > -1 && 
			worldModel->getBall().getPos().getX() < 17)
			basicPos[i].x = -1;

		if ((i == 1 || i == 4) && basicPos[i].x > 5) 
			basicPos[i].x = 4;

		if (i == 7 && basicPos[i].x > 4 && 
			worldModel->getBall().getPos().getX() < 25)
			basicPos[i].x = 4;

		if (i == 7 && basicPos[i].x > -1 && 
			worldModel->getBall().getPos().getX() < 17)
			basicPos[i].x = -1;

		if (i == 7 && basicPos[i].x > 8)
			basicPos[i].x = 8;
////////////////////THISO IFS
		if ((i <= 4 || i == 7) && worldModel->getBall().getPos().getX() > -28
			&& worldModel->getBall().getPos().getX() < 0)
		{
//			float unitStep = 6;
//			float deffStep = ((int)(worldModel->getBall().getPos().getX() / 
//						unitStep) - 1) * unitStep;
//			basicPos[i].x = deffStep;
		}
		
		if (i == 2 || i == 3)
		{
			float maxXFor34 = getMaxXFor34();
			if (worldModel->getBall().getPos().getX() > -38)
			{
				if (basicPos[i].x > maxXFor34 + ENSEJAM_MAX_X)
					basicPos[i].x = maxXFor34 + ENSEJAM_MAX_X;
			}
			else
			{
				if (basicPos[i].x > maxXFor34 + 7)
					basicPos[i].x = maxXFor34 + 7;
			}	
			LOG << "maxXFor3&4 : " << maxXFor34 << endl;
		}
		if (i == 7 && worldModel->getBall().getPos().getX() > -38)
		{
			float maxXFor7 = getMaxXFor7();
			float extraFor7 = 3.6;
			if (basicPos[i].x > maxXFor7 + extraFor7)
				basicPos[i].x = maxXFor7 + extraFor7;
			LOG << "maxXFor7 : " << maxXFor7 << endl;
		}
////////////////////THISO IFS


			 	
		float offsideLine = worldModel->getOppOffsideLine();
		LOG << "oppOffsideLine : " << offsideLine << endl;
		
		offsideLine -= 3.5;
				
		if ((i == 9 || i == 10)	&& 
			worldModel->getBall().getPos().getX() > 38)
		{
			basicPos[i].x = fmax(worldModel->getBall().getPos().getX(), 
								basicPos[i].x);
			basicPos[i].x = fmin(basicPos[i].x, 50);					
		}	 

		
		if (i == 10 && basicPos[i].y < -2)
			basicPos[i].y = -2;	
		if (i == 9 && basicPos[i].y > 2)
			basicPos[i].y = 2;
			
		if (basicPos[i].y > 32)
			basicPos[i].y = 32;
		if (basicPos[i].y < -32)
			basicPos[i].y = -32;
////////////////Akharin rooz : begin
		if (i == 9 && worldModel->getBall().getPos().getX() > 36 &&
			worldModel->getBall().getPos().getY() < -7)
		{
			float extraX;
			if (worldModel->getBall().getPos().getY() < -28)
				extraX = 7;
			else	
				extraX = Basics::reRate(
					fabs(worldModel->getBall().getPos().getY()), 7, 28, 0, 7); 
			basicPos[i].x -= extraX;
			if (basicPos[i].x < 36)
				basicPos[i].x = 36;
			basicPos[i].y -= 1.5;	
				
		}		
		
		if (i == 10 && worldModel->getBall().getPos().getX() > 36 &&
			worldModel->getBall().getPos().getY() > 7)
		{
			float extraX;
			if (worldModel->getBall().getPos().getY() > 28)
				extraX = 7;
			else	
				extraX = Basics::reRate(
					fabs(worldModel->getBall().getPos().getY()), 7, 28, 0, 7); 
			basicPos[i].x -= extraX;
			if (basicPos[i].x < 36)
				basicPos[i].x = 36;
			basicPos[i].y += 1.5;	
		}		
		if (i == 8 && worldModel->getBall().getPos().getX() > 40 &&
			fabs(worldModel->getBall().getPos().getY()) > 7)
		{
			float extraX;
			if (fabs(worldModel->getBall().getPos().getY()) > 25)
				extraX = 3;
			else	
				extraX = Basics::reRate(
						fabs(worldModel->getBall().getPos().getY()), 7, 25, 0, 3);
			basicPos[i].x -= extraX;
			if (basicPos[i].x < 36)
				basicPos[i].x = 36;
		}		
		
		if (i == 9 && worldModel->getBall().getPos().getX() > 36 &&
			worldModel->getBall().getPos().getY() > 5)
		{
			float extraX;
			if (worldModel->getBall().getPos().getY() > 15)
				extraX = 0;
			else
				extraX = Basics::reRate(
					fabs(worldModel->getBall().getPos().getY()), 5, 15, 0, 0);
			basicPos[i].x += extraX;
			if (basicPos[i].x > 51)
				basicPos[i].x = 51;
		}		
		if (i == 10 && worldModel->getBall().getPos().getX() > 36 &&
			worldModel->getBall().getPos().getY() < -5)
		{
			float extraX;
			if (worldModel->getBall().getPos().getY() < -15)
				extraX = 0;
			else
				extraX = Basics::reRate(
					fabs(worldModel->getBall().getPos().getY()), 5, 15, 0, 0); 
			basicPos[i].x += extraX;
			if (basicPos[i].x > 51)
				basicPos[i].x = 51;
		}		
////////////////Akharin rooz : end		

///////////////////////OFFENSE SECTION
		if (i == 5 && basicPos[i].y < 0 &&
 			worldModel->getBall().getPos().getX() > 35)
			basicPos[i].y = 0;

		if (i == 6 && basicPos[i].y > 0 &&
 			worldModel->getBall().getPos().getX() > 35)
			basicPos[i].y = 0;
///////////////////////OFFENSE SECTION

		if (i == 1 || i == 2 || i == 3 || i == 4 || i == 7)
			if (basicPos[i].x > -1)
				basicPos[i].x = -1; 
		
		if (basicPos[i].x > offsideLine && 
				(worldModel->getPlayMode() == PM_PLAY_ON))
			basicPos[i].x = offsideLine;
	}
}

Command *Positioning::getPositioningCommand(Point homePoint) const
{
	float maxDashPower = 100;		

	// Stamina Manager
/*	if (worldModel->getBody().getPostNum() == 5 || 
				worldModel->getBody().getPostNum() == 2)  
	{
		maxDashPower = 100;

		if (worldModel->getBody().getPos().getX() < homePoint.x && 
				homePoint.x > -36)
		{
			maxDashPower = 50;		
			if (worldModel->getBody().getStamina() < 2300)
				maxDashPower = worldModel->getBody().getStaminaIncMax();
		}
				
		if (worldModel->getBall().getPos().getX() > 0 && 
			!((worldModel->getBody().getPos().getX() - homePoint.x) > 4) && 
			worldModel->getBody().getStamina() < 3000)
			maxDashPower = worldModel->getBody().getStaminaIncMax() / 2; 
				
		if (worldModel->getBall().getPos().getX() > 15 && 
			!((worldModel->getBody().getPos().getX() - homePoint.x) > 6) && 
			worldModel->getBody().getStamina() < 3000)
			maxDashPower = worldModel->getBody().getStaminaIncMax() / 3; 
	}*/
	
	if (worldModel->getBody().getPostNum() == 4 || 
		worldModel->getBody().getPostNum() == 3 ||  
		worldModel->getBody().getPostNum() == 2 || 
		worldModel->getBody().getPostNum() == 5)  
{
		maxDashPower = 100;

		if (worldModel->getBody().getPos().getX() < homePoint.x && 
			homePoint.x > -36 && worldModel->getBall().getPos().getX() < 0)
		{