goalieintercept.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

			if (waitBall.getBodyVec().getMagnitude() <= (bodyCatchableArea + extraCatchableForNoise + extraCatchableForAngleNoise))
			{
				gtbPoint[gtbPointsNumber] = Point(waitBody.getPos().getX(), waitBody.getPos().getY()); 
				extraGTBTime[gtbPointsNumber] = i;
				gtbTime[gtbPointsNumber] = mainGoToBallTime + (extraGTBTime[gtbPointsNumber] / 2.00);
//				gtbTime[gtbPointsNumber] -= 2;				
				minBallDistance[gtbPointsNumber] = waitBall.getBodyVec().getMagnitude();
/*TOF2*/	gtbPointWeight[gtbPointsNumber] += min((bodyCatchableArea - minBallDistance[gtbPointsNumber]) , (float).3) * 10; 		
				distToBegin[gtbPointsNumber] = hypot(waitBall.getPos().getX() - worldModel->getBall().getPos().getX(), waitBall.getPos().getY() - worldModel->getBall().getPos().getY());
				delete gtbCommand[gtbPointsNumber];
				gtbCommand[gtbPointsNumber] = new DashCommand(AT_GOALIE_INTERCEPT, mainNegativeDashPower);
				catchedBall = 1;
				gtbPointsNumber++;
			}
			waitBody.simulateByDynamics();
			waitBall.simulateByDynamics(waitBody);
		}
		
		if (catchedBall) 
				catchedBallCycles++;
	}
//	LOG << "MeisamGoToBall : gtbPointsNumberT! : " << gtbPointsNumber << endl;
///////////////OneTurnPositiveDash
	
	Body virtualDashBody;
	virtualDashBody = simBody;
	virtualDashBody.setVel().setAsCartesian(0,0);
	Vector goToBallVector;
	float deltaAngleToGTB;
	float gtbRadius = bodyCatchableArea;
//	LogMaker.Log("GTBRadius 1 : ", GTBRadius); 
	simBody = worldModel->getBody();
	simBall = worldModel->getBall();
	catchedBallCycles = 0;
	catchedBall = 0;
	mainGoToBallTime = 0;
	extraCatchableForNoise = -.045 * mainGoToBallTime;
	simBody.simulateByDynamics();
	simBall.simulateByDynamics(simBody);
	goToBallVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
	deltaAngleToGTB = simBody.getBodyDir() - goToBallVector.getDirection();
	deltaAngleToGTB = Degree::normalizeAngle(deltaAngleToGTB);
//	LogMaker.Log("DeltaAngleToGTB 1 : ", DeltaAngleToGTB);
	mainGoToBallTime++;
//KOMAK	virtualDashBody.setStamina(virtualDashBody.setStamina() + virtualDashBody.getStaminaIncMax() * 1);/*One Turn*/;
	dashPower = 100;
	if (virtualDashBody.getStamina() < 1400)
		dashPower = virtualDashBody.getStaminaIncMax();
	tempDashCommand.setPower(dashPower);	
	while (mainGoToBallTime < GOALIE_MAX_GTB_CHECKED_TIME && gtbPointsNumber < 30)
	{
		mainGoToBallTime++;
//KOMAK		virtualDashBody.setStamina(virtualDashBody.getStamina() + virtualDashBody.getStaminaIncMax() * 1);/*One Turn*/;
		
		dashPower = 100;
		if (virtualDashBody.getStamina() < 1400)
				dashPower = virtualDashBody.getStaminaIncMax();
		tempDashCommand.setPower(dashPower);	
		virtualDashBody.simulateByAction(&tempDashCommand);
//KOMAK		virtualDashBody.setStamina(virtualDashBody.getStamina() - dashPower);
		gtbRadius += virtualDashBody.getVel().getMagnitude();
		
		extraCatchableForNoise = - .045 * mainGoToBallTime;
		extraCatchableForNoise -= .45;
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);
		goToBallVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
		deltaAngleToGTB = goToBallVector.getDirection() - simBody.getBodyDir();
		deltaAngleToGTB = Degree::normalizeAngle(deltaAngleToGTB);
		float deltaAngleToBallAngle;
		Vector gtbVector;
		gtbVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
		deltaAngleToBallAngle = gtbVector.getDirection() - simBall.getVel().getDirection();
		deltaAngleToBallAngle = Degree::normalizeAngle(deltaAngleToBallAngle);

		if (goToBallVector.getMagnitude() < 10)
		{
			if (fabs(deltaAngleToBallAngle) < 90)
//				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
			extraCatchableForAngleNoise = 0;
			
			else if (fabs(deltaAngleToBallAngle) < 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
			else if (fabs(deltaAngleToBallAngle) >= 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
		}
		else
		{
				if (fabs(deltaAngleToBallAngle) < 90)
//				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
			extraCatchableForAngleNoise = 0;
			else if (fabs(deltaAngleToBallAngle) < 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.8;
			else if (fabs(deltaAngleToBallAngle) >= 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.8;
		}

		if ((gtbRadius + extraCatchableForNoise  + extraCatchableForAngleNoise) >= goToBallVector.getMagnitude() && fabs(deltaAngleToGTB) <= BasicTurn::getTurnAngle(179,worldModel->getBody())) 
		{
			gtbPoint[gtbPointsNumber] = Point(simBody.getPos().getX(), simBody.getPos().getY()); 
			gtbTime[gtbPointsNumber] = mainGoToBallTime;
			extraGTBTime[gtbPointsNumber] = 0;
			minBallDistance[gtbPointsNumber] = goToBallVector.getMagnitude() - gtbRadius + bodyCatchableArea;
			distToBegin[gtbPointsNumber] = hypot(simBall.getPos().getX() - worldModel->getBall().getPos().getX(), simBall.getPos().getY() - worldModel->getBall().getPos().getY());
				delete gtbCommand[gtbPointsNumber]; 
			gtbCommand[gtbPointsNumber] = TurnAngle(AT_GOALIE_INTERCEPT, deltaAngleToGTB, worldModel->getBody()).getCommand();
/*TOF*/			if (fabs(deltaAngleToGTB) <= 8)
{
				delete gtbCommand[gtbPointsNumber]; 
/*TOF*/				gtbCommand[gtbPointsNumber] = new DashCommand(AT_GOALIE_INTERCEPT, mainPositiveDashPower);
}
			gtbPointsNumber++;
		}	
	}
	
//	LOG << "MeisamGoToBall : gtbPointsNumber 2 : " << gtbPointsNumber << endl;


///////////////OneTurnNegativeDash
	
/*	virtualDashBody = simBody;
	virtualDashBody.setVel().setAsCartesian(0,0);
	gtbRadius = bodyCatchableArea;
	simBody = worldModel->getBody();
	simBall = worldModel->getBall();
	catchedBallCycles = 0;
	catchedBall = 0;
	mainGoToBallTime = 0;
	extraCatchableForNoise = .06 * mainGoToBallTime;
	simBody.simulateByDynamics();
	simBall.simulateByDynamics(simBody);
	goToBallVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
	deltaAngleToGTB = simBody.getBodyDir() - goToBallVector.getDirection();
	deltaAngleToGTB = Degree::normalizeAngle(deltaAngleToGTB);
	mainGoToBallTime++;
//KOMAK	virtualDashBody.setStamina(virtualDashBody.getStamina() + virtualDashBody.getStaminaIncMax() * 1);
	dashPower = -100;
	if (virtualDashBody.getStamina() < 1400)
		dashPower = -virtualDashBody.getStaminaIncMax() / 2.00;
   tempDashCommand.setPower(dashPower);	
	while (mainGoToBallTime < GOALIE_MAX_GTB_CHECKED_TIME && gtbPointsNumber < 50)
	{
		mainGoToBallTime++;
//KOMAK		virtualDashBody.setStamina(virtualDashBody.getStamina() + virtualDashBody.getStaminaIncMax() * 1);
		
		dashPower = -100;
		if (virtualDashBody.getStamina() < 1400)
				dashPower = -virtualDashBody.getStaminaIncMax() / 2.00;
		tempDashCommand = DashCommand(dashPower);	
		virtualDashBody.simulateByAction(&dashCommand);
//KOMAK				virtualDashBody.setStamina(virtualDashBody.getStamina() + DashPower * 2);
		gtbRadius += virtualDashBody.getVel().getMagnitude();
		
		extraCatchableForNoise = .06 * mainGoToBallTime;
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);
		goToBallVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
		goToBallVector.setAsPolar(goToBallVector.getMagnitude(), Degree::normalizeAngle(goToBallVector.getDirection() + 180));
		deltaAngleToGTB = goToBallVector.getMagnitude() - simBody.getBodyDir();
		deltaAngleToGTB = Degree::normalizeAngle(deltaAngleToGTB);

//KOMAK		if((gtbRadius + extraCatchableForNoise) >= goToBallVector.getMagnitude() && fabs(deltaAngleToGTB) <= calculate.Public.GetVirtualTurnAngle(179)) 
		{
			gtbPoint[gtbPointsNumber] = Point(simBody.getPos().getX(), simBody.getPos().getY()); 
			gtbTime[gtbPointsNumber] = mainGoToBallTime;
			extraGTBTime[gtbPointsNumber] = 0;
			minBallDistance[gtbPointsNumber] = goToBallVector.getMagnitude() - gtbRadius + bodyCatchableArea;
			distToBegin[gtbPointsNumber] = hypot(simBall.getPos().getX() - worldModel->getBall().getPos().getX(), simBall.getPos().getY() - worldModel->getBall().getPos().getY());
			gtbCommand[gtbPointsNumber] = *TurnAngle(deltaAngleToGTB, worldModel->getBody());
			gtbPointsNumber++;
		}	
	}*/
//	LOG << "MeisamGoToBall : gtbPointsNumber 3 : " << gtbPointsNumber << endl;

///////////////TwoTurnPositiveDash
	
	virtualDashBody = simBody;
	virtualDashBody.setVel().setAsCartesian(0,0);
	gtbRadius = bodyCatchableArea;
	simBody = worldModel->getBody();
	simBall = worldModel->getBall();
	catchedBallCycles = 0;
	catchedBall = 0;
	mainGoToBallTime = 0;
	extraCatchableForNoise = - .045 * mainGoToBallTime;
	for (int i = 0; i < 2; i++)
	{
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);
		goToBallVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
		deltaAngleToGTB = simBody.getBodyDir() - goToBallVector.getDirection();
		deltaAngleToGTB = Degree::normalizeAngle(deltaAngleToGTB);
		mainGoToBallTime++;
//KOMAK				virtualDashBody.setStamina(virtualDashBody.getStamina() + virtualDashBody.getStaminaIncMax());
	}
	dashPower = 100;
	if (virtualDashBody.getStamina() < 1400)
		dashPower = virtualDashBody.getStaminaIncMax();
	tempDashCommand.setPower(dashPower);	
	while (mainGoToBallTime < GOALIE_MAX_GTB_CHECKED_TIME && gtbPointsNumber < 70)
	{
		mainGoToBallTime++;
//		virtualDashBody.setStamina()(virtualDashBody.getStamina() + virtualDashBody.getStaminaIncMax());
		
		dashPower = 100;
		if (virtualDashBody.getStamina() < 1400)
				dashPower = virtualDashBody.getStaminaIncMax();
		tempDashCommand.setPower(dashPower);	
		virtualDashBody.simulateByAction(dynamic_cast <Command *> (&tempDashCommand));
//KOMAK				virtualDashBody.setStamina(virtualDashBody.getStamina() - dashPower);
		gtbRadius += virtualDashBody.getVel().getMagnitude();
		
		extraCatchableForNoise = -.045 * mainGoToBallTime;
		extraCatchableForNoise -= .45;
		simBody.simulateByDynamics();
		simBall.simulateByDynamics(simBody);
		goToBallVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
		deltaAngleToGTB = goToBallVector.getDirection() - simBody.getBodyDir();
		deltaAngleToGTB = Degree::normalizeAngle(deltaAngleToGTB);
		float deltaAngleToBallAngle;
		Vector gtbVector;
		gtbVector.setByPoints(Point(simBody.getPos().getX(), simBody.getPos().getY()), Point(simBall.getPos().getX(), simBall.getPos().getY()));
		deltaAngleToBallAngle = gtbVector.getDirection() - simBall.getVel().getDirection();
		deltaAngleToBallAngle = Degree::normalizeAngle(deltaAngleToBallAngle);
		if (goToBallVector.getMagnitude() < 10)
		{
			if (fabs(deltaAngleToBallAngle) < 90)
//				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
			extraCatchableForAngleNoise = 0;
			else if (fabs(deltaAngleToBallAngle) < 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
			else if (fabs(deltaAngleToBallAngle) >= 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
		}
		else
		{
				if (fabs(deltaAngleToBallAngle) < 90)
//				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1;
			extraCatchableForAngleNoise = 0;
			else if (fabs(deltaAngleToBallAngle) < 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.8;
			else if (fabs(deltaAngleToBallAngle) >= 120)
				extraCatchableForAngleNoise = (Degree::cos(fabs(deltaAngleToBallAngle)) + 0) * 1.8;
		}

		if ((gtbRadius + extraCatchableForNoise + extraCatchableForAngleNoise) >= goToBallVector.getMagnitude())// && fabs(deltaAngleToGTB) <= Calculate.Public.GetVirtualTurnAngle(180)) 
		{
			gtbPoint[gtbPointsNumber] = Point(simBody.getPos().getX(), simBody.getPos().getY()); 
			gtbTime[gtbPointsNumber] = mainGoToBallTime;
			extraGTBTime[gtbPointsNumber] = 0;
			minBallDistance[gtbPointsNumber] = goToBallVector.getMagnitude() - gtbRadius + bodyCatchableArea;
			distToBegin[gtbPointsNumber] = hypot(simBall.getPos().getX() - worldModel->getBall().getPos().getX(), simBall.getPos().getY() - worldModel->getBall().getPos().getY());
				delete gtbCommand[gtbPointsNumber];
			gtbCommand[gtbPointsNumber] = TurnAngle(AT_GOALIE_INTERCEPT, deltaAngleToGTB, worldModel->getBody()).getCommand();
/*TOF*/			if (fabs(deltaAngleToGTB) <= 8)
{
				delete gtbCommand[gtbPointsNumber];
/*TOF*/				gtbCommand[gtbPointsNumber] = new DashCommand(AT_GOALIE_INTERCEPT, mainPositiveDashPower);
}
			gtbPointsNumber++;
		}	
	}

//	LOG << "MeisamGoToBall : gtbPointsNumber 4 : " << gtbPointsNumber << endl;
	for (int i = 0; i < gtbPointsNumber; i++)
	{
		gtbPointWeight[i] -= gtbTime[i];
//		gtbPointWeight[i] += extraGTBTime[i];	
	}
	
	bestGTBPointNumber = 0;
	for (int i = 0; i < gtbPointsNumber; i++)
	{
//		LOG << "i , gtbTime[i] : " <<  i << " " << gtbTime[i] << endl;
//		LOG << "i , gtbWeightPoint[i] : " << i << " " <<  gtbPointWeight[i] << endl;
//		LOG << "i , extraGTBTime[i] : " << i << " " << extraGTBTime[i] << endl;
//		LOG << "i , minBallDistance[i] : " << i << " " << minBallDistance[i] << endl;
		if (gtbPointWeight[i] > gtbPointWeight[bestGTBPointNumber])
			bestGTBPointNumber = i; 
	}
	
	if (dynamic_cast <DashCommand *> (gtbCommand[bestGTBPointNumber]) != NULL && 
		worldModel->getBall().getVel().getMagnitude() > .4 && 
		worldModel->isOppBallShooted(worldModel->getBall()))
	{

		float deltaDirToBallVel;
		deltaDirToBallVel = worldModel->getBall().getVel().getDirection() - 
							worldModel->getBody().getBodyDir();
		deltaDirToBallVel = Degree::normalizeAngle(deltaDirToBallVel);
		deltaDirToBallVel = fabs(deltaDirToBallVel);
		if (deltaDirToBallVel < 170 && deltaDirToBallVel > 10)
		{
			Line bodyLine, ballLine;
			Point interPoint;
			ballLine.setBySourceDir(worldModel->getBall().getPos(), 
					worldModel->getBall().getVel().getDirection());
			bodyLine.setBySourceDir(worldModel->getBody().getPos(), 
					worldModel->getBody().getBodyDir());
			ballLine.getLineIntersect(bodyLine, interPoint);
			delete gtbCommand[bestGTBPointNumber];
			gtbCommand[bestGTBPointNumber] = PreciseGotoPoint(AT_GOALIE_INTERCEPT, interPoint, .2,
						worldModel->getBody(), 1).getCommand();
		}
	} 
	if (gtbPoint[bestGTBPointNumber].x > -35.5 ||
		fabs(gtbPoint[bestGTBPointNumber].y) > 19.5)
		return AD_MIN_VALUE;
	else
		return AD_ALWAYS_RUN_VALUE;	
//		LOG << "BestGTBCommand : " << gtbCommand[bestGTBPointNumber]->toString() << endl;		
//	LOG << "BestGTBPointNumber : " << bestGTBPointNumber << endl;

	return AD_NORMAL_VALUE;
}

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

	command = gtbCommand[bestGTBPointNumber];
}