interception1.cpp

robocup3d源化码 Nexus3D.tar.gz

/*
Copyright (C) 2005 Ferdowsi University
*/


#include "interception.h"
#include "global.h"

Intercept::Intercept()
{
 
}

bool Intercept::TryIntercept()
{
	Vector3f target;
	target[2] = self.pos[2];
	if((ball.vel.Length() > 0.5) && ball.distance > 0.5){
		float min_dist=200;
		/*
		if((self.home_pos-predicted_ball_pos[stop_index]).Length() > MyPlayer[MyNumber].move_radius)
		{
			DoLog(LOG_INTERCEPT,"1.out of my move_radius, L=%.2f, R=%.2f",(self.home_pos-predicted_ball_pos[stop_index]).Length(),MyPlayer[MyNumber].move_radius);
			return false;
		}
		*/

		for(int i = 1; i<12; i++)
		{
			if(i != MyNumber && (MyPlayer[i].home_pos-predicted_ball_pos[stop_index]).Length() < min_dist)
			{
				min_dist = (MyPlayer[i].home_pos-predicted_ball_pos[stop_index]).Length();
		  		DoLog(LOG_INTERCEPT,"1.MyPlayer[%d] is closer",i);
			}
		}
		if((self.home_pos-predicted_ball_pos[stop_index]).Length() > min_dist)
		{
	  		DoLog(LOG_INTERCEPT,"1.somebody is closer");
			return false;
		}
		DoLog(LOG_INTERCEPT,"using prediction,interception to (%.2f,%.2f),    ball.pos=(%.2f,%.2f)",predicted_ball_pos[stop_index][0],predicted_ball_pos[stop_index][1],ball.pos[0],ball.pos[1]);
		target[0] = predicted_ball_pos[stop_index][0];
		target[1] = predicted_ball_pos[stop_index][1];
	}
	else{
		float min_dist=200;
		/*
		if((self.home_pos - ball.pos).Length() > MyPlayer[MyNumber].move_radius)
		{
			DoLog(LOG_INTERCEPT,"2.out of my move_radius, L=%.2f, R=%.2f",(self.home_pos - ball.pos).Length(),MyPlayer[MyNumber].move_radius);
			return false;
		}
		*/
		for(int i = 1; i<12; i++)
		{
			if(i != MyNumber && (MyPlayer[i].home_pos - ball.pos).Length() < min_dist)
			{
				min_dist = MyPlayer[i].dist2ball + (MyPlayer[i].home_pos - ball.pos).Length();
		  		DoLog(LOG_INTERCEPT,"2.MyPlayer[%d] is closer",i);
			}
		}
		if((self.home_pos - ball.pos).Length() > min_dist)
		{
	  		DoLog(LOG_INTERCEPT,"2.somebody is closer");
			return false;
		}
  		DoLog(LOG_INTERCEPT,"interception to (%.2f,%.2f)",ball.pos[0],ball.pos[1]);
		target[0] = ball.pos[0];
		target[1] = ball.pos[1];
	}

	if((self.pos - target).Length() > 1 && ball.vel.Length() < 4)
	{
		float angle = NormalizeAngle(Rad2Deg(atan2((0 - target[1]),(FieldLength/2 - target[0]))),-180.0);
		Vector3f pos = AdjustKickPos.MoveAroundBall(angle);
		/*
		target[0] -= 0.3 * cos(Deg2Rad(angle));
		target[1] -= 0.3 * sin(Deg2Rad(angle));
		*/
		target[0] = pos[0];
		target[1] = pos[1];
		DoLog(LOG_INTERCEPT,"interception Adjusted. target = (%.2f,%.2f)",target[0],target[1]);
	}

	action_info.x = target[0];
	action_info.y = target[1];
	action_info.do_drive = true;
	return true;
}