pass.cpp

robocup3d源化码 Nexus3D.tar.gz

/*
Copyright (C) 2005 Ferdowsi University
*/

#include "pass.h"
#include "global.h"
#include "math.h"

Pass::Pass()
{
	//  initializing
	for (int i=0;i<10;i++)
	{
		DirectPassInfo[i].confidence = 0.0;
		DirectPassInfo[i].angle = 10.0;
		DirectPassInfo[i].direction = 0.0;
		DirectPassInfo[i].speed = 0.0;
		DirectPassInfo[i].pos = Vector3f(0,0,0);
		DirectPassInfo[i].PlayerNumber = 1;
	}
}

void Pass::GeneratePossibleDirectPasses()
{
	int i,j=0,k=0;
	for (i=1; i<12; i++)
		if (i != MyNumber){
			DirectPassInfo[j].pos[0] = MyPlayer[i].pos[0] + 3;
			DirectPassInfo[j].pos[1] = MyPlayer[i].pos[1];
			DirectPassInfo[j].speed = (ball.pos - DirectPassInfo[i-1].pos).Length() * 10/3;  // power to kick ball
			if(DirectPassInfo[j].speed > 100.0) DirectPassInfo[j].speed = 100;
			DirectPassInfo[j].direction = NormalizeAngle(Rad2Deg(atan2(ball.pos[1] - DirectPassInfo[j].pos[1] , ball.pos[0] - DirectPassInfo[j].pos[0])),-180);
			//if(DirectPassInfo[j].pos[0] < ball.pos[0]) DirectPassInfo[j].direction += 180;
			DirectPassInfo[j].direction += 180;
			/*
			if ( fabs(ball.pos[0] - DirectPassInfo[j].pos[0]) < 0.01)
				if (ball.pos[1] > DirectPassInfo[j].pos[1])
					DirectPassInfo[j].direction = -90;
				else
					DirectPassInfo[j].direction = 90;
			else
				DirectPassInfo[j].direction = Rad2Deg(atan ( (ball.pos[1] - DirectPassInfo[j].pos[1]) / (ball.pos[0] - DirectPassInfo[j].pos[0]) ));
			*/
			DirectPassInfo[j].PlayerNumber = i;
			j++;
		}
}

PassInfo Pass::EvaluateBestDirectPass()
{
	GeneratePossibleDirectPasses();
	// calculating positions distance from pass sender
	int i, j;
	float DistToMe[10];
	for (i=0;i<10;i++)
		DistToMe[i] = (self.pos - DirectPassInfo[i].pos).Length();
	// calculating number of oponents in Tradious around the selected points
	int OponentAround[10]={0};
	int Tradious = 10;
	for (i=0;i<10;i++)
		for (j=1;j<12;j++)
			if((DirectPassInfo[i].pos - TheirPlayer[j].pos).Length() < Tradious)
				OponentAround[i] ++;
	// calculating posipassable!tions distance to the oponent goal center
	float DistToGoal[10];
	for (int i=0;i<10;i++)
		DistToGoal[i] = (Vector3f(FieldLength/2,0,AgentRadius) - DirectPassInfo[i].pos).Length();
	// calculating positions' confidence to pass
	float reachability;
	Vector3f ball_vel = Vector3f(0,0,0);
	bool interceptable;
	float MaxDistToPass = 22;
	// angle is the needed turn angle
	float angle = NormalizeAngle(Rad2Deg(atan2((self.pos[1]-ball.pos[1]),(self.pos[0]-ball.pos[0]))),-180.0);
	for (i=0;i<10;i++){
		DirectPassInfo[i].confidence = 0;
		if (DirectPassInfo[i].pos[0] > - FieldLength/4)
		{
			ball_vel[0] = cos(DirectPassInfo[i].direction) * DirectPassInfo[i].speed / 10 ;
			ball_vel[1] = sin(DirectPassInfo[i].direction) * DirectPassInfo[i].speed / 10;
			interceptable = OpponentCanIntercept(ball_vel);
			if(interceptable)
			{
				DirectPassInfo[i].angle = 30;
				reachability = 0.75;
				DoLog(LOG_PASS, "DirectPassing by dircetion %.2f is interceptable", DirectPassInfo[i].direction);
			}
			if(DistToMe[i] < MaxDistToPass)
				reachability = 1;
			else
				reachability = 0;

			DirectPassInfo[i].confidence = 1 / (
							  DistToMe[i] / MaxDistToPass
							+ OponentAround[i] / 11
							+ 5*DistToGoal[i] / FieldLength
							+ 15*fabs(angle-DirectPassInfo[i].direction) / 180
							) * reachability;
		}
		DoLog(LOG_PASS, "DirectPassing... to player %d : confidence = %.2f, dircetion = %.2f, angle = %.2f, speed = %.2f, pos = (%.2f,%.2f)", DirectPassInfo[i].PlayerNumber, DirectPassInfo[i].confidence, DirectPassInfo[i].direction, DirectPassInfo[i].angle, DirectPassInfo[i].speed, DirectPassInfo[i].pos[0], DirectPassInfo[i].pos[1]);
	}

	// evaluating best positions' confidence to pass
	int Best = 0;
	float Max = DirectPassInfo[0].confidence;
	for (i=0;i<10;i++)
		if (DirectPassInfo[i].confidence>Max)
		{
			Best = i;
			Max = DirectPassInfo[Best].confidence;
		}
	return DirectPassInfo[Best];
}


bool Pass::CanPass()
{
	BestDirectPass = EvaluateBestDirectPass();
	if(BestDirectPass.confidence > 0)
	{
		DoLog(LOG_PASS, "passable!");
		return true;
	}
	DoLog(LOG_PASS, "Can NOT Pass -> no confidence");
	return false;
}

PassInfo Pass::GetBestPass()
{
	if(BestDirectPass.confidence > 0){
		DoLog(LOG_PASS, "Best Direct Passing... to player %d : confidence = %.2f, dircetion = %.2f, angle = %.2f, speed = %.2f, pos = (%.2f,%.2f)", BestDirectPass.PlayerNumber ,BestDirectPass.confidence, BestDirectPass.direction, BestDirectPass.angle, BestDirectPass.speed, BestDirectPass.pos[0], BestDirectPass.pos[1]);
		return BestDirectPass;
	}
	PassInfo temp;
	return temp;
}