adjustkickpos.cpp

robocup3d源化码 Nexus3D.tar.gz

/*
	|No|Copyright (C) 2005 Ferdowsi University
*/

#include "adjustkickpos.h"
#include "global.h" 

AdjustKickPosition::AdjustKickPosition()
{
}
int AdjustKickPosition::Sign(float x)
{
	if (x<0) return -1;
	else if(x>0) return +1;
	else return 0;
}
//receives an object(object2turn) to turn around it and an angle(DestAngle) that final position of agent make with that object,
//and  distance(dist) of final position of agent with object
// the agent will be in a place that it can see the object with (DestAngle) angle

Vector3f AdjustKickPosition::FinalPos(float DestAngle, float dist)//Entry is in Degree
{
	Vector3f Final;
	Final[0]=-dist*cos(Deg2Rad(DestAngle));
	Final[1]=-dist*sin(Deg2Rad(DestAngle));
	DoLog(LOG_ADJUST,"Dest Angle=%.2f, dist=%.2f",DestAngle,dist);
	DoLog(LOG_ADJUST,"final pos =(%.2f %.2f)",Final[0],Final[1]);
	Final[2]=self.pos[2];
	return Final;
}

Vector3f AdjustKickPosition::MoveAroundBall(float kickAngle)
{
	const float view=70;
	Vector3f nextPos;//final result for position of Agent in next Server-Cycle will save here
	float agentAngle;
	float turnAngle;
	float tempAngle1= 0.00;
	float tempAngle2=0.00;

	if(fabs(ball.vel.Length())>0.5 && ball.distance>0.5){
		agentAngle=NormalizeAngle(Rad2Deg(atan2((predicted_ball_pos[stop_index][1]-self.pos[1]),(predicted_ball_pos[stop_index][0]-self.pos[0]))),-180.0);
		//DoLog(LOG_ADJUST,"agentangle1=%f",agentAngle);
		turnAngle=NormalizeAngle(agentAngle-kickAngle,-180.0);
		//DoLog(LOG_ADJUST,"turnAngle1=%.2f",turnAngle);

		if(fabs(turnAngle)<view)
		{
			nextPos=FinalPos(kickAngle,MinKickDistance-0.2/*0.04*/);
			DoLog(LOG_ADJUST,"Turn Not Needed1");
		}//************************************New Added codes***********************************
		else// if(ball.distance> 0.40 )
		{
			tempAngle1= 0.00;
			tempAngle2= 0.00;
			Vector3f tempPos1;
			Vector3f tempPos2;
			tempAngle1=NormalizeAngle(agentAngle-(90),-180.0);
			tempAngle2=NormalizeAngle(agentAngle+(90),-180.0);

			tempPos1=FinalPos(tempAngle1,MinKickDistance+0.05);
			tempPos2=FinalPos(tempAngle2,MinKickDistance+0.05);
			if((FinalPos(kickAngle,MinKickDistance)-tempPos1).Length()<(FinalPos(kickAngle,MinKickDistance)-tempPos2).Length()) nextPos=tempPos1;
			else nextPos=tempPos2;
			DoLog(LOG_ADJUST,"Turn Needed1");
		}
		DoLog(LOG_ADJUST,"predicted_ball_pos[stop_index]=(%.2f,%.2f)",predicted_ball_pos[stop_index][0],predicted_ball_pos[stop_index][1]);
		nextPos[0] += predicted_ball_pos[stop_index][0];
		nextPos[1] += predicted_ball_pos[stop_index][1];
	}
	else{

		agentAngle=NormalizeAngle(Rad2Deg(atan2((ball.pos[1]-self.pos[1]),(ball.pos[0]-self.pos[0]))),-180.0);
		//DoLog(LOG_ADJUST,"agentangle2=%f",agentAngle);
		turnAngle=NormalizeAngle(agentAngle-kickAngle,-180.0);
		//DoLog(LOG_ADJUST,"turnAngle2=%.2f",turnAngle);

		if(fabs(turnAngle)<view)
		{
			nextPos=FinalPos(kickAngle,MinKickDistance-0.2);
			DoLog(LOG_ADJUST,"Turn Not Needed2");
		}//************************************New Added codes***********************************
		else// if(ball.distance> 0.40 )
		{
			tempAngle1= 0.00;
			tempAngle2= 0.00;
			Vector3f tempPos1;
			Vector3f tempPos2;
			tempAngle1=NormalizeAngle(agentAngle-(90),-180.0);
			tempAngle2=NormalizeAngle(agentAngle+(90),-180.0);

			tempPos1=FinalPos(tempAngle1,MinKickDistance+0.05);
			tempPos2=FinalPos(tempAngle2,MinKickDistance+0.05);
			if((FinalPos(kickAngle,MinKickDistance)-tempPos1).Length()<(FinalPos(kickAngle,MinKickDistance)-tempPos2).Length()) nextPos=tempPos1;
			else nextPos=tempPos2;
			DoLog(LOG_ADJUST,"Turn Needed2");
		}
		nextPos[0] += ball.pos[0];
		nextPos[1] += ball.pos[1];
	}
	DoLog(LOG_ADJUST,"nextPos = (%.2f,%.2f), ball.pos = (%.2f,%.2f)",nextPos[0],nextPos[1],ball.pos[0],ball.pos[1]);
	return nextPos;
}

bool AdjustKickPosition::AdjustNeeded(float kickAngle, float epsilon)
{
	kickAngle = NormalizeAngle(kickAngle,-180.0);
	float agentAngle=atan2((ball.pos[1]-self.pos[1]),(ball.pos[0]-self.pos[0]));
	agentAngle=NormalizeAngle(Rad2Deg(agentAngle),-180.0);
	DoLog(LOG_ADJUST,"kickangle = %.2f, agentangle = %.2f, epsilon = %.2f",kickAngle,agentAngle,epsilon);
	if(fabs(agentAngle-kickAngle) < epsilon && self.dist2ball < MinKickDistance)
	{
		DoLog(LOG_ADJUST,"***********Adjust NOT needed!**************, self.dist2ball = %.2f",self.dist2ball);
	 	return false;
	}
	else
	{
		DoLog(LOG_ADJUST,"************Adjust needed!**************, self.dist2ball = %.2f",self.dist2ball);
		return true;
	}
}