/* *  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 <Logger.h>#include <BasicDash.h>#include <BasicTurn.h>#include <Positioning.h>#include <Basics.h>#include <cmath>using namespace std;Positioning::Positioning(const WorldModel *worldModel):		worldModel(worldModel){	coorseVar = false;	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(-44, 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, 10);	playerPos[1][7] = Point(-25, 0);	playerPos[1][6] = Point(-20, -10);	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, 16);	playerPos[3][2] = Point(14, -5.8);	playerPos[3][3] = Point(14, 5.8);	playerPos[3][1] = Point(14, -16);	playerPos[3][5] = Point(20, 10);	playerPos[3][7] = Point(15, 0);	playerPos[3][6] = Point(20, -10);	playerPos[3][8] = Point(24, 0);	playerPos[3][9] = Point(23, -20);	playerPos[3][10] = Point(23, 20);	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));	}	attraction[7] = .35;	ballPos[7] = Point(52.5, 0);	playerPos[7][4] = Point(10, 15);	playerPos[7][2] = Point(0, -10);	playerPos[7][3] = Point(0, 10);	playerPos[7][1] = Point(10, -15);	playerPos[7][5] = Point(50, 15);	playerPos[7][7] = Point(17, 0);	playerPos[7][6] = Point(50, -15);	playerPos[7][8] = Point(49, 0);	playerPos[7][9] = Point(48, -(+40));	playerPos[7][10] = Point(48, -(-40));	attraction[4] = .35;	ballPos[4] = Point(24, 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, 15);	playerPos[5][2] = Point(0, -10);	playerPos[5][3] = Point(0, 10);	playerPos[5][1] = Point(10, -15);	playerPos[5][5] = Point(35, 10);	playerPos[5][7] = Point(17, 0);	playerPos[5][6] = Point(35, -10);	playerPos[5][8] = Point(51, 0);	playerPos[5][9] = Point(50, -3);	playerPos[5][10] = Point(50, 3);}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++)	{	LOG << "MEISAM2005 Positioning aaaaah : REGION :  " << region << endl;		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;//ghame ma'navi	  if (i == 7 && basicPos[i].x > 4 && //ghame ma'navi			worldModel->getBall().getPos().getX() < 25)//ghame ma'navi			basicPos[i].x = 4;//ghame ma'navi		if (i == 7 && basicPos[i].x > -1 && //ghame ma'navi			worldModel->getBall().getPos().getX() < 17)//ghame ma'navi			basicPos[i].x = -1;//ghame ma'navi		if (i == 7 && basicPos[i].x > 8)//ghame ma'navi			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 == 3)			LOG << "MEISAM2005 positioning: basicPos[3] A : " << basicPos[i] << endl;		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)		{//ghame ma'navi			float maxXFor7 = getMaxXFor7();//ghame ma'navi			float extraFor7 = 3.6;//ghame ma'navi			if (basicPos[i].x > maxXFor7 + extraFor7)//ghame ma'navi				basicPos[i].x = maxXFor7 + extraFor7;//ghame ma'navi			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)			if (basicPos[i].x > -1)				basicPos[i].x = -1; 		//Meisam2005	if (i == 1 && worldModel->getBall().getPos().getX() > 15)	{		basicPos[i].x += worldModel->getBall().getPos().getX() * .2;		if (worldModel->getBall().getPos().getY() >= 0)			basicPos[i].x -= fabs(worldModel->getBall().getPos().getY()) * .2;		if (worldModel->getBall().getPos().getY() < 0)			basicPos[i].x += fabs(worldModel->getBall().getPos().getY()) * .2;		if (basicPos[i].x < -1)			basicPos[i].x = -1;		LOG << "Meisam2005 Positioning :  basicPos[1]" << basicPos[i].x << endl; 		}	if (i == 4 && worldModel->getBall().getPos().getX() > 15)	{		basicPos[i].x += worldModel->getBall().getPos().getX() * .2;		if (worldModel->getBall().getPos().getY() < 0)			basicPos[i].x -= fabs(worldModel->getBall().getPos().getY()) * .2;		if (worldModel->getBall().getPos().getY() >= 0) 			basicPos[i].x += fabs(worldModel->getBall().getPos().getY()) * .2;		if (basicPos[i].x < -1)			basicPos[i].x = -1;		LOG << "Meisam2005 Positioning :  basicPos[4]" << basicPos[i].x << endl; 		}//Meisam2005		if (i == 3)		LOG << "MEISAM2005 positioning: basicPos[3] B : " << basicPos[i] << endl;	if (i == 8 && worldModel->getBall().getPos().getX() < 36)	{		basicPos[i].y += min(worldModel->getBall().getPos().getY() * .3 , 6.00);		basicPos[i].y = min(basicPos[10].y - 7 , basicPos[i].y);		basicPos[i].y = max(basicPos[9].y + 7 , basicPos[i].y);			}	if (i >= 8)	{		if (coorseVar == false)		{			if (basicPos[i].x - worldModel->getBody().getPos().getX() > 8 &&				worldModel->getBall().getPos().getX() > 0 &&				abs(worldModel->getBody().getPos().getY() - 					basicPos[i].y) < 8)			{				coorseVar = true;				basicPos[i].y = worldModel->getBody().getPos().getY();				LOG << "Meisam2005 coorseVar 1 " << endl;			}				}		else		{			if (basicPos[i].x - worldModel->getBody().getPos().getX() > 6 &&				worldModel->getBall().getPos().getX() > 0 &&				abs(worldModel->getBody().getPos().getY() - 					basicPos[i].y) < 10)			{				coorseVar = true;				basicPos[i].y = worldModel->getBody().getPos().getY();				LOG << "Meisam2005 coorseVar 2 " << endl;			}				}	}	if (i == 5 || i == 6)	{		if (coorseVar == false)		{			if (basicPos[i].x - worldModel->getBody().getPos().getX() > 6 &&				worldModel->getBall().getPos().getX() > 0 &&				abs(worldModel->getBody().getPos().getY() - 					basicPos[i].y) < 8)			{				coorseVar = true;				basicPos[i].y = worldModel->getBody().getPos().getY();				LOG << "Meisam2005 coorseVar 1 " << endl;			}				}		else		{			if (basicPos[i].x - worldModel->getBody().getPos().getX() > 4 &&				worldModel->getBall().getPos().getX() > 0 &&				abs(worldModel->getBody().getPos().getY() - 					basicPos[i].y) < 10)			{				coorseVar = true;				basicPos[i].y = worldModel->getBody().getPos().getY();				LOG << "Meisam2005 coorseVar 2 " << endl;			}				}	}