strategy.cpp

robocup源代码2001年清华机器人源代码

/******************      Field  ***************************/
Field::Field(){
}

void Field::Initialize(){

	field = Rect(-SP_semi_pitch_length, SP_semi_pitch_length,
		-SP_semi_pitch_width,SP_semi_pitch_width);
	l_field = Rect(-SP_semi_pitch_length, SP_semi_pitch_length,
		0, -SP_semi_pitch_width);
	r_field = Rect(-SP_semi_pitch_length, SP_semi_pitch_length,
		0, SP_semi_pitch_width);

	backfield = Rect(-SP_semi_pitch_length, SP_backfieldline,
		-SP_semi_pitch_width,SP_semi_pitch_width);
	forwardfield = Rect(SP_midfieldline, SP_semi_pitch_length,
		-SP_semi_pitch_width, SP_semi_pitch_width);
	midfield = Rect(SP_backfieldline, SP_midfieldline,
		-SP_semi_pitch_width, SP_semi_pitch_width);

	l_backfield = Rect(-SP_semi_pitch_length, SP_backfieldline, 
		-SP_semi_pitch_width, SP_left_wing);
	m_backfield = Rect(-SP_semi_pitch_length, SP_backfieldline,
		SP_left_wing, SP_right_wing);
	r_backfield = Rect(-SP_semi_pitch_length, SP_backfieldline,
		SP_right_wing, SP_semi_pitch_width);

	l_midfield = Rect(SP_backfieldline, SP_midfieldline,
		-SP_semi_pitch_width, SP_left_wing);
	m_midfield = Rect(SP_backfieldline, SP_midfieldline,
		SP_left_wing,SP_right_wing);
	r_midfield = Rect(SP_backfieldline, SP_midfieldline,
		SP_right_wing, SP_semi_pitch_width);

	l_forwardfield = Rect(SP_midfieldline, SP_semi_pitch_length,
		-SP_semi_pitch_width, SP_left_wing);
	m_forwardfield = Rect(SP_midfieldline, SP_semi_pitch_length,
		SP_left_wing, SP_right_wing);
	r_forwardfield = Rect(SP_midfieldline, SP_semi_pitch_length,
		SP_right_wing, SP_semi_pitch_width);

	my_penaltyarea = Rect(-SP_semi_pitch_length, SP_penalty_area_length -SP_semi_pitch_length,
		SP_penalty_area_width /2, - SP_penalty_area_width /2);
	their_penaltyarea = Rect(SP_semi_pitch_length, - SP_penalty_area_length + SP_semi_pitch_length,
		SP_penalty_area_width /2, - SP_penalty_area_width /2);

	my_goaltenderbox = Rect(-SP_semi_pitch_length, -SP_semi_pitch_length + SP_goal_area_length,
		SP_goal_area_width / 2, - SP_goal_area_width / 2);

	their_goaltenderbox = Rect(SP_semi_pitch_length, SP_semi_pitch_length - SP_goal_area_length,
		SP_goal_area_width / 2, - SP_goal_area_width / 2);

	my_goaldefendbox = Rect(-SP_semi_pitch_length, -SP_semi_pitch_length + SP_goal_area_length - SP_catch_area_l /2, 
		SP_goal_area_width / 2, - SP_goal_area_width / 2); 

	shootablearea = Rect(SP_semi_pitch_length, SP_semi_pitch_length - CP_max_shoot_distance,
		-SP_semi_pitch_width, SP_semi_pitch_width);

	expanded_field = Rect(-SP_semi_pitch_length - CP_out_of_pitch_error, SP_semi_pitch_length + CP_out_of_pitch_error,
		-SP_semi_pitch_width - CP_out_of_pitch_error, SP_semi_pitch_width + CP_out_of_pitch_error);

	shrinked_field = Rect(-SP_semi_pitch_length + CP_out_of_pitch_error, SP_semi_pitch_length - CP_out_of_pitch_error,
		-SP_semi_pitch_width + CP_out_of_pitch_error, SP_semi_pitch_width - CP_out_of_pitch_error);

	mygoal = Vector(-SP_semi_pitch_length, 0);
	theirgoal = Vector(SP_semi_pitch_length, 0);

	l_mygoalpost = Vector(-SP_semi_pitch_length, -SP_goal_width / 2.0f);
	r_mygoalpost = Vector(-SP_semi_pitch_length, SP_goal_width / 2.0f);
	l_theirgoalpost = Vector(SP_semi_pitch_length, -SP_goal_width / 2.0f);
	r_theirgoalpost = Vector(SP_semi_pitch_length, SP_goal_width / 2.0f);

	SideLines[SL_Right].LineFromTwoPoints(Vector(SP_semi_pitch_length, 1), Vector(SP_semi_pitch_length, 0));
	SideLines[SL_Left].LineFromTwoPoints(Vector(-SP_semi_pitch_length, 1), Vector(-SP_semi_pitch_length, 0));
	SideLines[SL_Bottom].LineFromTwoPoints(Vector(1, SP_semi_pitch_width), Vector(0, SP_semi_pitch_width));
	SideLines[SL_Top].LineFromTwoPoints(Vector(1, -SP_semi_pitch_width), Vector(0, -SP_semi_pitch_width));

	theirgoalie = -1;
	if(Self.Is_goalie)
		mygoalie = MyNumber;
	else if(FP_goalie_number == MyNumber)
		mygoalie = -1;
	else
		mygoalie = FP_goalie_number;
}

bool Field::IsInvalidZone(Vector Pos){
	switch (situation.playmode){
	case PM_My_Kick_Off:
		return (Pos.x>0);
		break;
	case PM_Their_Kick_Off:
		return (Pos.x>0||ball.pos_valid()&&Pos.dist(ball.pos)<SP_free_kick_buffer);
		break;
	case PM_Their_Goal_Kick :
			return their_penaltyarea.IsWithin(Pos);
			break;
	case PM_Their_Kick_In :
	case PM_Their_Corner_Kick :
	case PM_Their_Free_Kick :
	case PM_Their_Offside_Kick :
		return (Pos.dist(ball.pos)<SP_free_kick_buffer);
		break;
	default :
		return false;
		break;
	}
}

float Field::GetOffsideLine(){
	if(offsideline.IsDataKnown(situation.CurrentTime))	
		return offsideline.Data(situation.CurrentTime);
	
	float offsidex = 0;
	UNum offsidep = -1;
	
	int oppgoalie = fieldinfo.IsValidtheirgoalie() ? fieldinfo.theirgoalie  :  1;
	for(int i = 1; i <= SP_team_size; i ++){
		if(i == oppgoalie) continue;
		if(TheirPlayer(i).Rs_valid() || TheirPlayer(i).pos_valid()){
			if(TheirPlayer(i).pos.x > offsidex){
				offsidex = TheirPlayer(i).pos.x;
				offsidep = i;
			}
		}
	}
	
	float ballposx = ball.pos.x;
	if(situation.BallFree && ball.pos_valid() && ball.Rs_valid())
		ballposx = Max(ballposx, situation.NextControlBallPos().x - 2.0f);
	
	offsideline.Setdata(Max(offsidex, ballposx), situation.CurrentTime);
	if(offsidep > 0) Behindestopp.Setdata(offsidep, situation.CurrentTime);
	
	return offsideline.Data(situation.CurrentTime);
}

bool Field::IsOffside(){
	return bool(Self.pos.x > (GetOffsideLine() - 1.0f));
}

bool Field::Maybeoffside(){
	if(IsOffside()) return true;

	if(!Susoffsideopp.IsDataKnown(situation.CurrentTime)){
		Susoffsideopp.Setdata(GuessOffside(), situation.CurrentTime);
	}

	return bool(Susoffsideopp.Data(situation.CurrentTime) > 0);
}

UNum Field::GuessOffside(){
	if(Self.pos.x <= 0 || Self.pos.x < ball.pos.x) return -1;

	UNum goalieno = fieldinfo.IsValidtheirgoalie() ? fieldinfo.theirgoalie : 1;
	goalieno += SP_team_size;
	UNum susopp = -1;
	for(int i = 0; i < motion.Num_TheirVisiblePlayers(); i++){
		if(motion.TheirPlayer_InFront(i).InsideNO == goalieno) continue;

		if(motion.TheirPlayer_InFront(i).pos.x < Self.pos.x - 10.0f) 
			return susopp;

		if(motion.TheirPlayer_InFront(i).pos.x - motion.TheirPlayer_InFront(i).ActiveRadius(situation.CurrentTime) > Self.pos.x) 
			return -1;		

		if(motion.TheirPlayer_InFront(i).pos.x + motion.TheirPlayer_InFront(i).ActiveRadius(situation.CurrentTime) <= Self.pos.x)
			continue;
		
		if(susopp < 0) susopp = motion.TheirPlayers_InFront[i];
	}
	return susopp;
}

float Field::GetOffensiveSensitivity(Vector pos){
	sensitivity_input[0] = pos.x / 52.5;
	sensitivity_input[1] = fabs(pos.y) / 35;
	OffSensitivityNet.SimulateNet(&sensitivity_input[0], &sensitivity_output);
	return (float)(0.5f + 0.5f * sensitivity_output * 1.111f);
}

float Field::GetDefensiveSensitivity(Vector pos){
	sensitivity_input[0] = pos.x / 52.5;
	sensitivity_input[1] = fabs(pos.y) / 34;
	DF_SensitivityNet.SimulateNet(&sensitivity_input[0], &sensitivity_output);
	return float((sensitivity_output - 0.1) * 1.25);
}

bool Field::WithInField(Vector pos){
	return field.IsWithin(pos);
}

bool Field::WithInExpandedField(Vector pos){
	return expanded_field.IsWithin(pos);
}

bool Field::WithInLeftField(Vector pos){
	return l_field.IsWithin(pos);
}

bool Field::WithInRightField(Vector pos){
	return r_field.IsWithin(pos);
}

bool Field::WithInShootableArea(Vector pos){
	return shootablearea.IsWithin(pos);
}

int Field::NumOpponentsInCone(AngleDeg direction, float distance, AngleDeg angle_range){
	int num = motion.Num_TheirVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if (motion.TheirPlayer_Close2Me(i).distance > distance)
			break;
		else{
			if (fabs(NormalizeAngle(motion.TheirPlayer_Close2Me(i).global_angle - direction)) < angle_range / 2.0f){
				count ++;
			}
		}
	}
	return count;
}

int Field::NumOpponentsInCone(Vector pos, AngleDeg direction, float distance, AngleDeg angle_range){
	int num = motion.Num_TheirVisiblePlayers();
	int count = 0;
	Vector gap;
	for(int i = 0; i < num; i ++){
		gap = motion.TheirPlayer_Close2Me(i).pos - pos;
		if (gap.mod2() < Sqr(distance) && fabs(gap.Angle() - direction) <= angle_range /2)
				count ++;
	}
	return count;
}

int Field::NumTeammatesInCone(AngleDeg direction, float distance, AngleDeg angle_range){
	int num = motion.Num_MyVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if (motion.MyPlayer_Close2Me(i).distance > distance)
			break;
		else{
			if (fabs(NormalizeAngle(motion.MyPlayer_Close2Me(i).global_angle - direction)) < angle_range / 2.0f){
				count ++;
			}
		}
	}
	return count;	
}


int Field::NumTeammatesInCone(Vector pos, AngleDeg direction, float distance, AngleDeg angle_range){
	int num = motion.Num_MyVisiblePlayers();
	int count = 0;
	Vector gap;
	for(int i = 0; i < num; i ++){
		gap = motion.MyPlayer_Close2Me(i).pos - pos;
		if (gap.mod2() < Sqr(distance) && fabs(NormalizeAngle(gap.Angle() - direction)) <= angle_range /2)
			count ++;
	}
	return count;
}

int Field::NumNoneGoalieTeammatesInCone(Vector end1, Vector end2, AngleDeg angle_range){
	Vector polar = end2 - end1;
	return NumNoneGoalieTeammatesInCone(end1, polar.Angle(), polar.mod(), angle_range);
}

int Field::NumNoneGoalieTeammatesInCone(Vector pos, AngleDeg direction, float distance, AngleDeg angle_range){
	int num = motion.Num_MyVisiblePlayers();
	int count = 0;
	Vector gap;
	for(int i = 0; i < num; i ++){
		if (motion.MyPlayers_Close2Me[i] == FP_goalie_number) continue;
		gap = motion.MyPlayer_Close2Me(i).pos - pos;
		if (gap.mod2() < Sqr(distance) && fabs(NormalizeAngle(gap.Angle() - direction)) <= angle_range /2)
			count ++;
	}
	return count;
}

int Field::NumOpponentsWithin(float distance){
	int num = motion.Num_TheirVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if (motion.TheirPlayer_Close2Me(i).distance > distance)
			break;
		count ++;
	}
	return count;
}

int Field::NumTeammatesWithin(float distance){
	int num = motion.Num_MyVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if (motion.MyPlayer_Close2Me(i).distance > distance)
			break;
		count ++;
	}
	return count;
}

int Field::NumOpponentsWithin(float Dist, AngleDeg angle, float ofDist, AngleDeg ofangle){
	int num = motion.Num_TheirVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if (motion.TheirPlayer_Close2Me(i).distance > ofDist)
			break;
		if (motion.TheirPlayer_Close2Me(i).distance > Dist 
			&& Angle_between(motion.TheirPlayer_Close2Me(i).global_angle, angle, ofangle)){
			count ++;
		}
	}
	return count;
}

int Field::NumTeammatesWithin(float Dist, AngleDeg angle, float ofDist, AngleDeg ofangle){
	int num = motion.Num_MyVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if (motion.MyPlayer_Close2Me(i).distance > ofDist)
			break;
		if (motion.MyPlayer_Close2Me(i).distance > Dist 
			&& Angle_between(motion.MyPlayer_Close2Me(i).global_angle, angle, ofangle)){
			count ++;
		}
	}
	return count;
}

int Field::NumOpponentsWithin(float distance, Vector pos){
	int num = motion.Num_TheirVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if ((motion.TheirPlayer_Close2Me(i).pos - pos).mod2() < Sqr(distance))
			count ++;
	}
	return count;
}

int Field::NumTeammatesWithin(float distance, Vector pos){
	int num = motion.Num_MyVisiblePlayers();
	int count = 0;
	for(int i = 0; i < num; i ++){
		if ((motion.MyPlayer_Close2Me(i).pos - pos).mod2() < Sqr(distance))
			count ++;
	}
	return count;
}


bool Field::IsBlocked(UNum No){
	if (IsTheirPlayer(No)){
		return IsBlocked(situation.TheirSide, No - SP_team_size);
	}
	else if (IsMyPlayer(No)){
		return IsBlocked(situation.MySide, No);
	}
	return false;
}

bool Field::IsBlocked(char side, UNum No){ 
	if (situation.TheirSide == side)
	{
		Vector OPos = TheirPlayer(No).pos;
		AngleDeg l_goal_ang = (fieldinfo.l_mygoalpost - OPos).Angle();
		AngleDeg r_goal_ang = (fieldinfo.r_mygoalpost - OPos).Angle();
		AngleDeg m_ang = MidAngle2(l_goal_ang,r_goal_ang);
		return (fieldinfo.NumTeammatesInCone(OPos,m_ang,CP_block_dist,AngleDif(l_goal_ang,r_goal_ang)) > 0);
	}
	else{
		if (MyPlayer(No).pos_valid()){
			float angle = (fieldinfo.theirgoal - MyPlayer(No).pos).Angle();
			return bool(fieldinfo.NumNoneGoalieTeammatesInCone(MyPlayer(No).pos, angle, CP_block_dist, 120.0f) > 0);
		}
	}
	return false;
}

float Field::FitPosDegree(Vector CurrentPos, Vector TargetPos, float radius){
	if (radius<1.0f) radius=1.0f;
	return (TargetPos.dist(CurrentPos) < radius ? 1 : 
              1/(1+Sqr(TargetPos.dist(CurrentPos)/radius-1.0f) * 4.0f));	
}

 float Field::FitPosDegree(UNum No, Vector TargetPos, float radius){
	return FitPosDegree(MyPlayer(No).pos, TargetPos, radius);
}

float Field::CalcThreat(UNum No){
	//estimate threat of an opponent
	if (!IsTheirPlayer(No)) return 0.0f;
	return TheirPlayer(No).GetDefsensitivity();
}

/*** The3rdCurve ********/

The3rdCurve::The3rdCurve(float a, float b, float c, float d, float inf, float sup){
	SetABCD(a, b, c, d);
	SetInfSup(inf, sup);
}

void The3rdCurve::SetABCD(float a, float b, float c, float d){
	this->a = a;
	this->b = b;
	this->c = c;
	this->d = d;
}

void The3rdCurve::SetInfSup(float inf, float sup){
	this->inf = inf;
	this->sup = sup;
}

float The3rdCurve::GetOutput(float x, bool limited){
	float result = a *x * x *x + b * x * x + c * x + d;
	if (limited){
		pose_limitation(result, inf, sup);
	}
	return result;
}

bool The3rdCurve::Interpolation(float x0, float y0, float x1, float y1, float x2, float y2, float x3, float y3){
	if (x0 == x1 || x0 == x2 || x0 == x3 || x1 == x2 || x1 == x3 || x2 == x3) return false;
	float t0, t1, t2, t3;
	t0 = y0 / ((x0 - x1) * (x0 - x2) * (x0 - x3));
	t1 = y1 / ((x1 - x0) * (x1 - x2) * (x1 - x3));
	t2 = y2 / ((x2 - x0) * (x2 - x1) * (x2 - x3));
	t3 = y3 / ((x3 - x0) * (x3 - x1) * (x3 - x2));

	a = t0 + t1 + t2 + t3;
	b = - (t0 * (x1 + x2 + x3) + t1 * (x0 + x2 + x3) + t2 * (x0 + x1 + x3) + t3 * (x0 + x1 +x2));
	c = t0 * (x1 * x2 + x1 * x3 + x2 * x3) + t1 * (x0 * x2 + x0 * x3 + x2 * x3)
		+ t2 * (x1 * x0 + x3 * x1 + x3 * x0) + t3 * (x0 * x1 + x0 * x2 + x1 * x2);
	d = -(t0 * (x1 * x2 * x3) + t1 * (x0 * x2 * x3) + t2 * (x0 * x1 * x3) + t3 * (x0 * x1 * x2));
	return true;
}

/******************** Place Kick Arrangement ***********/
void PlaceKick::CheckHeadingAndStamina()
//check if the executer is going to the ball properly
{	UNum i;
	Vector meanvel,balldir;

	for (i=2;i<=SP_team_size;i++)
	{
	if (Trackmark[i].x>SP_pitch_length/2 || !MyPlayer(i).pos_valid()) 
	{
		HeadingToBall[i] = 1.0f - 2.0f * (situation.CurrentTime - situation.ModeChangedTime)/ SP_drop_ball_time;
	}
	else
	{
		meanvel=MyPlayer(i).pos-Trackmark[i];
		balldir=ball.pos-MyPlayer(i).pos;
		if (balldir.mod()<1.0f)
			HeadingToBall[i]=1.0f;
		else
		{
			balldir.Normalize();
			HeadingToBall[i]=
				((meanvel.x*balldir.x+meanvel.y*balldir.y)/CP_setplay_speed+1)/2;
		}
	}
	pose_limitation(HeadingToBall[i],0,1);
	if (MyPlayer(i).pos_valid())
				Trackmark[i]=MyPlayer(i).pos;
	else 
				Trackmark[i]=Vector(SP_pitch_length,SP_pitch_width);
	}

}

UNum PlaceKick::JudgeExecutor()
{	
	Vector meanvel,balldir;
	UNum i,select=MyNumber;
	float min_pos_dis,min_cur_dis,player_i_pos_dis,player_i_cur_dis;
	Vector ofpos;
	if (situation.IsGoalieKick) return FP_goalie_number;
	if (ball.kickable()) return MyNumber;
	if (IsMyPlayer(situation.ballcontroller)&&(fieldinfo.mygoalie != situation.ballcontroller))
		return situation.ballcontroller;
	if (Counter%6==0&&Counter > 0) CheckHeadingAndStamina();
	ofpos=fm.GetOffensivePoint(Self.attackness, Self.leftness, ball.pos);
	min_pos_dis=ofpos.dist(ball.pos);
	min_cur_dis=ball.distance;
	for (i=2;i<=SP_team_size;i++)
		if (i!=MyNumber)
	{
		if (HeadingToBall[i]<0.75) continue;