df_positioning.cpp

RoboCup仿真组世界冠军源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "df_positioning.h"
#include "global.h"



/***********	DF_Arrangement	***********************/
/*      generate subtasks for defense scheme          */
DF_Arrangement::DF_Arrangement(UNum defender, UNum defendee){
	df_size =  Vector(CP_defense_area_length, CP_defense_area_width);
	valid = Arrange_DF_for(defender, defendee);
}

/*
  press : when the opponent carrying ball is ahead of you so that you cannot
  block, try dashing back hard to keep threatening him
*/
bool DF_Arrangement::getpressposition(UNum My,UNum Opp,Vector &presspos){  
	if (fieldinfo.IsBlocked(defendee))
	{
		presspos = ball.pos;
		return true;
	}
	
	Ray theircourse;
	Vector l_mark,r_mark;
	Vector sectionpost,theirtarget;
	Vector MyPos = MyPlayer(My).pos;
	Vector Opos = TheirPlayer(Opp).pos;
	if (MyPos.dist(fieldinfo.mygoal) > Opos.dist(fieldinfo.mygoal))
		Opos += Polar2Vector(TheirPlayer(Opp).ActiveRadius(situation.CurrentTime) + TheirPlayer(Opp).max_speed * 2,
		(fieldinfo.mygoal - TheirPlayer(Opp).pos).Angle());
	//assume opponent's movement
	
	Line splitline;
	
	Line me_Opp;
	me_Opp.LineFromTwoPoints(MyPos,Opos);
	Vector goalpnt = me_Opp.intersection(fieldinfo.SideLines[SL_Left]);
	if (fabs(goalpnt.y) > SP_goal_width/2 * 1.05f)
	{
		l_mark = fieldinfo.l_mygoalpost;
		r_mark = fieldinfo.r_mygoalpost;
	}
	else
	{
		if (goalpnt.y < 0)
		{
			l_mark = fieldinfo.mygoal;
			r_mark = fieldinfo.r_mygoalpost;
		}
		else
		{
			l_mark = fieldinfo.l_mygoalpost;
			r_mark - fieldinfo.mygoal;
		}
	}
	splitline.LineFromTwoPoints((MyPos + Opos)/2,(l_mark + r_mark) /2);
	if (splitline.HalfPlaneTest(Opos) == splitline.HalfPlaneTest(l_mark))
	{
		sectionpost = l_mark;
		theirtarget = r_mark;
	}
	else 
	{
		sectionpost = r_mark;
		theirtarget = l_mark;
	}
	theircourse.SetValues(Opos,(theirtarget - Opos).Angle());
	Line mycourse;
	sectionpost = (sectionpost + Opos) /2; 
	mycourse.LineFromTwoPoints(MyPos,sectionpost);
	if  (!(theircourse.intersection(mycourse,presspos) && fieldinfo.WithInField(presspos)))
	{
		DoLog(LOG_BUG,"no press point 1");
		return false;
	}
	if (presspos.dist(MyPos) *1.1 < presspos.dist(Opos) && AngleDif((presspos - MyPos).Angle(),(presspos - Opos).Angle()) > 60
		&& presspos.dist(Opos) > 2.5f)
	{
		splitline.LineFrompline(MyPos,Opos);
		if (!theircourse.intersection(splitline,presspos))
		{
			DoLog(LOG_BUG,"no press point 2");
			return false;
		}
	}
	//consideration of bodyfacing
	Ray SelfRay(Self.pos,Self.bodyfacing);
	Vector secpoint;
	float secdist;
	float pressdist = theircourse.DistanceFromOrigin(theirtarget);
	if (theircourse.intersection(SelfRay,secpoint))
	{
		secdist = secpoint.dist(presspos);
		if (secdist < 1.0f || secdist < 0.15f * pressdist)
			presspos = secpoint;
	}
	return true;
}

/*
	block: intercept opponent carrying ball
*/
bool DF_Arrangement::getblockposition(UNum My, UNum Opp, Vector &blockpos){ 
	if (!IsTheirPlayer(Opp) || !IsMyPlayer(My)) return false;
	float cycles = 0.0f;
	Vector opppos;
	if (situation.BallFree){
		opppos = TheirPlayer(Opp).IT_inf.IT_point;
		cycles = TheirPlayer(Opp).IT_inf.IT_cycles;
	}
	else{
		opppos = TheirPlayer(Opp).pos;
		cycles = 0.0f;
	}
	float my_block_speed = SP_player_speed_max;
	float their_break_speed = SP_player_speed_max; 
 	if (!getblockpoint(opppos, MyPlayer(My).pos, cycles, my_block_speed, their_break_speed, blockpos))
		return false;
	if (!fieldinfo.WithInField(blockpos)) return false;
	float dsens = fieldinfo.GetDefensiveSensitivity(blockpos);
	if ( dsens > 0.95f && dsens > 1.1f * MyPlayer(My).defsensitivity.Data(situation.CurrentTime))
		return false;
	return true;
}

bool DF_Arrangement::getblockpoint(Vector opp_IT_pos, Vector selfpos, float opp_IT_cycles, float myspeed, float oppspeed, Vector& blockpos){
	float predis = myspeed * opp_IT_cycles;

	float disleft = opp_IT_pos.dist(fieldinfo.l_mygoalpost);
	float ratio = disleft/(disleft + opp_IT_pos.dist(fieldinfo.r_mygoalpost));
	Vector opttarget = Vector(-SP_pitch_length/2, SP_goal_width * (ratio - 0.5f));
	Vector opponent_pos = (opttarget - opp_IT_pos);
	opponent_pos.Normalize();
	opponent_pos = opp_IT_pos + opponent_pos * CP_block_margin;

	Vector vec_me_opt = selfpos - opponent_pos ;
	float dis_me_opt = vec_me_opt.mod();

	if(dis_me_opt <= predis || oppspeed < 1e-2){
		blockpos = opponent_pos;
		return true;
	}
	
	float ratio_speed = myspeed / oppspeed;
	
	if(fabs(NormalizeAngle(vec_me_opt.Angle()-(opttarget-opponent_pos).Angle()))<10)
		return getsubpoint(opttarget,opponent_pos,selfpos,ratio_speed,predis,blockpos);

	Vector subpos[3];
	bool block_ok[3];
	block_ok[0] = getsubpoint(opttarget,opponent_pos,selfpos,ratio_speed,predis,subpos[0]);
	block_ok[1] = getsubpoint(fieldinfo.l_mygoalpost,opponent_pos,selfpos,ratio_speed,predis,subpos[1]);	
	block_ok[2] = getsubpoint(fieldinfo.r_mygoalpost,opponent_pos,selfpos,ratio_speed,predis,subpos[2]);
	for(int i=1;i<3;i++){
		if(subpos[i].x<subpos[0].x){
			subpos[0]=subpos[i];
			block_ok[0]=block_ok[i];
		}
	}
	blockpos=subpos[0];
	return block_ok[0];	
}

bool DF_Arrangement::getsubpoint(Vector opttarget, Vector opponent_pos, Vector selfpos, float ratio_speed,float predis, Vector& blockpos){
	Vector vec_me_opt = selfpos - opponent_pos ;
	float dis_me_opt = vec_me_opt.mod();

	float interangle = vec_me_opt.Angle()-(opttarget-opponent_pos).Angle();
	float tempf,tempc;
	float tempa = 1.0f - ratio_speed * ratio_speed;
	if(fabs(tempa)<1e-2){
		tempf = predis + dis_me_opt * Cos(interangle);

		if(tempf <= 1e-2){
			blockpos = (opponent_pos + opttarget)/2;
			return false;
		}

		tempc = 0.5f*(dis_me_opt * dis_me_opt - predis*predis) / tempf;
	}
	else{
		tempf = 2*predis*ratio_speed + 2*dis_me_opt * Cos(interangle);
		float tempz = tempf * tempf - 4 * tempa*(dis_me_opt*dis_me_opt-predis*predis);
		if(tempz < 0){
			blockpos = (opponent_pos+opttarget)/2;
			return false;
		}
		tempz = (float)sqrt(tempz);
		float tempx1 = 0.5f * (-tempz + tempf)/tempa;
		float tempx2 = 0.5f * (tempz + tempf)/tempa;
		if(tempx1 < 0 && tempx2 < 0){
			blockpos = (opponent_pos + opttarget)/2;
			return false;
		}
		else if(tempx1 > 0 && tempx2 > 0) tempc = Min(tempx1, tempx2);
		else tempc = Max(tempx1,tempx2);	
	}
	float odist = opponent_pos.dist(opttarget);

	if(tempc >= odist + 1e-2){
		 blockpos = (opponent_pos + opttarget)/2;
		 return false;
	}
	
	float ratio = tempc / odist;

	blockpos = (opponent_pos * (1-ratio) + opttarget * ratio);
	
	return true;
}

/*
	mark : defend on opponents without ball,staying on the way that more
	convenience to get ball
*/
bool DF_Arrangement::getmarkposition(UNum My, UNum Opp, Vector ballpos, Vector& markpos){ 
//here global numbering scheme is assumed
	if (!IsTheirPlayer(Opp) || !IsMyPlayer(My)) return false;
	Vector dir1, dir2;
	Vector OppPos = TheirPlayer(Opp).pos;
	Vector target = ballpos.y>0 ? fieldinfo.r_mygoalpost : fieldinfo.l_mygoalpost;
	float dispersion =TheirPlayer(Opp).ActiveRadius(situation.CurrentTime) + TheirPlayer(Opp).max_speed;
	if (AngleDif((target - OppPos).Angle(),(target - Self.pos).Angle()) > 90)
		dispersion += 2.0f;
	else
		dispersion += 1.0f;
	pose_limitation(dispersion,0.0f,5.0f);
	float sensfactor = motion.sens_pos_factor.GetOutput(2*(fieldinfo.GetDefensiveSensitivity(OppPos) -0.5f));
	pose_limitation(sensfactor,0,1);
	OppPos += Polar2Vector(dispersion* (1 - sensfactor),(target - OppPos).Angle());	

	dir1 = ballpos - OppPos;
	dir1.Normalize();
	dir2 = fieldinfo.mygoal - OppPos;
	dir2.Normalize();
	dir1 *= sensfactor;
	dir2 *= 1 - sensfactor;
	float	markdist = CP_mark_dist;
	if(MyPlayer(My).IsRoleKnown()){
	//if a player's aggressiveness is high, he may act more aggressively when marking the opponent.
	//for instance, he will stand just in front of the opponent
		if (MyPlayer(My).aggressiveness >= 0.7f){
			markpos = dir1;
		}
		else if (MyPlayer(My).aggressiveness >= 0.5f){
			markpos = dir1 * 0.8f + dir2 * 0.2f;
		}
		else{
			markpos = dir1 * 0.5f + dir2 * 0.5f;
		}
	}
	else{
		markpos = dir1;
	}

	markpos.Normalize();
	markpos *= markdist;
	//consider body facing
	Ray markRay(OppPos,markpos);
	Ray SelfRay(Self.pos,Self.bodyfacing);
	Vector secpoint;
	float secdist;
	float balldist = ballpos.dist(OppPos);
	if (markRay.intersection(SelfRay,secpoint))
	{
		secpoint -=OppPos;
		secdist = secpoint.dist(markpos);
		if (secdist < 0.5f || secdist < 0.1f * markdist)
			markpos = secpoint;
	}
	markpos += OppPos;
	return true;
}

void DF_Arrangement::setpriority(){ 
	switch(df_type){
	case DF_Block:
		  priority = blockpriority = getblockpriority();
		  break;
	case DF_Formation:
		  priority = formationpriority = getformationpriority();
		  break;
	case DF_Mark:
		  priority = markpriority = getmarkpriority();
		  break;
	case DF_Press:
		if (fieldinfo.IsBlocked(defendee))
			presspriority = getblockpriority();
		else
			presspriority = getpresspriority();
			blockpriority = getblockpriority();
		priority = blockpriority;
		break;
	default:
		break;
	}
}

float DF_Arrangement::QueryPriority(bool block_applied){ 
	if (df_type == DF_Press && block_applied)
	{
		return presspriority;
	}
	else
		return priority;
}
float DF_Arrangement::getformationpriority(){
	double input,output;
	pos_dist = df_point.dist(MyPlayer(defender).pos);
	deviation_dist = df_point.dist(fm_pos);
	input = fieldinfo.GetDefensiveSensitivity(fm_pos);
	motion.FM_Priority_Net.SimulateNet(&input,&output);
	return (float)output;
}

float DF_Arrangement::getblockpriority(){
	double input[3], output;
	pos_dist = TheirPlayer(defendee).pos.dist(MyPlayer(defender).pos);
	deviation_dist = Min(TheirPlayer(defendee).pos.dist(fm_pos), df_point.dist(fm_pos));
	input[0] = threat;
	input[1] = deviation_dist / CP_deviation_max;
	input[2] = pos_dist / CP_deviation_max;