positioning.cpp

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

/*
    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 "positioning.h"
#include "global.h"

/**************	PositionAction	**************/
PositionAction::PositionAction(){
}

void PositionAction::position(){
	Command command;
	if (point.dist2(Self.pos) > Sqr(max_deviation)){
		if (!fieldinfo.IsInvalidZone(point)){
			if(rapidness <= CP_slow_formation_speed && Self.IsStrained()){
				float controlspeed;
				if(Self.stamina < 2000) 
					controlspeed = 0;
				else
					controlspeed = rapidness * Self.stamina / SP_stamina_max;
				motion.go_to(point,  controlspeed, command);
				DoLog("position %.2f %.2f %.2f", point.x, point.y, controlspeed);
			}else{
				motion.go_to(point, rapidness, command);
				DoLog("position %.2f %.2f %.2f", point.x, point.y, rapidness);
			}
		}
	}
	else{
		command.stay();
	}
	mediator.enroll(command, actiontype, priority);
}

void PositionAction::SetPositioningInfo(Vector point, float rapidness, float max_deviation, float priority, ActionType actiontype){
	this->point = point;
	this->rapidness = rapidness;
	this->max_deviation = max_deviation;
	this->priority = priority;
	this->actiontype = actiontype;
}
/**************	Positioning	**************************/
Positioning::Positioning(){
//	sens_pos_factor.Interpolation(0.05f,0.2f, 0.7f,0.5f,0.9f,0.7f,0.95f,0.9f);
}

void Positioning::Defense(){
	if (ball.pos_conf < 0.8f) return;
	DF_Arrangement arrangement;
	if (Self.IsForward() && fieldinfo.IsOffside())
	{
		arrangement.df_type = DF_Formation;
		arrangement.df_point = Vector(fieldinfo.GetOffsideLine() - 1.5f, Self.pos.y);
		arrangement.valid = true;
		DoLog(LOG_OFFSIDE,"offside %.2f", fieldinfo.GetOffsideLine());
	}
	else if(Self.IsForward() && fieldinfo.Maybeoffside()){
		arrangement.df_type = DF_Formation;
		arrangement.df_point = Self.pos;
		arrangement.valid = true;
		DoLog(LOG_OFFSIDE,"suspicous offside %d", fieldinfo.Susoffsideopp.Data(situation.CurrentTime));
	}
	else
	{
		if (!MakeDefenseDecision()) return;
		 arrangement = GetAssignment();
	}
	
	if (!arrangement.valid)
		return;

	ActionType actiontype;

	switch(arrangement.df_type){
	case DF_Block:
		actiontype = Action_positioning_block;
		CloseBlock(arrangement);
		break;
	case DF_Mark:
		actiontype = Action_positioning_mark;
		break;
	case DF_Formation:
		actiontype = Action_positioning_defense;
		break;
	case DF_Press:
		actiontype = Action_press; 
		break;
	default:
		actiontype = Action_none;
	}
	logdefense.Setdata(arrangement, situation.CurrentTime);
	
	DoLog(LOG_POSITIONING, "DF point(%.2f %.2f)", arrangement.df_point.x, arrangement.df_point.y);
	SetPositioningInfo(arrangement.df_point, GetDefenseSpeed(arrangement), 0.5f, PriorityA, actiontype);
	position();
}

void Positioning::Offense(){
	if (Self.attackness <= 0.3f && situation.playmode == PM_Play_On)
		DefensiveFormation();
	else
		OffensiveFormation();
}

void Positioning::DefensiveFormation(){
	if(ball.pos_conf < 0.8f) return;//check if the information of ball is sufficient to make the decision
	Vector defensive_pos;
	defensive_pos = fm.GetDefensivePoint(Self.attackness, Self.leftness, situation.NextControlBallPos());
	
	SetPositioningInfo(defensive_pos, CP_slow_formation_speed, CP_pos_deviation_max, PriorityB, Action_positioning_defense);

	position();
}

void Positioning::OffensiveFormation(){
	if(ball.pos_conf < 0.8f) return;
	float offensespeed, deviation;
	Vector offensepos;
			
	if (fieldinfo.IsOffside()) 
	{
		offensepos = Vector(fieldinfo.GetOffsideLine() - 2.0f, Self.pos.y);
		offensespeed =  CP_fast_formation_speed;
		deviation = 0.5f;
		DoLog(LOG_OFFSIDE,"offside %.2f", fieldinfo.GetOffsideLine());
	}
	else if(fieldinfo.Maybeoffside()){
		offensepos = Self.pos;
		deviation = 1.0f;
		DoLog(LOG_OFFSIDE,"suspicous offside %d", fieldinfo.Susoffsideopp.Data(situation.CurrentTime));
	}
	else{
		offensepos = fm.GetOffensivePoint(Self.attackness, Self.leftness, situation.NextControlBallPos());
		
		if (offensepos.x > fieldinfo.GetOffsideLine()){
			offensepos.x = fieldinfo.GetOffsideLine() - 0.5f;
		}
		else{
			float t;
			if ((t = fm.GetOffensivePoint(0.9f, 0.5f, situation.NextControlBallPos()).x - fieldinfo.GetOffsideLine()) > 3.0f){
				//offensepos.x -=  0.4f * t;
			}
		}
		if (situation.gamestate != GS_Playing){
			offensespeed = CP_slow_formation_speed;
		}
		else
		 if (Self.attackness >= 0.8f){
			offensespeed = CP_fast_formation_speed * 0.9f;
		}
		else if (Self.attackness > 0.5f){
			offensespeed = CP_mid_formation_speed;
		}
		else{
			offensespeed = CP_slow_formation_speed;
		}
		deviation = CP_pos_deviation_max;
	} 
	SetPositioningInfo(offensepos, offensespeed, deviation, PriorityB, Action_positioning_offense);
	position();
}

float Positioning::GetDefenseSpeed(const DF_Arrangement & arrangement){
	switch(arrangement.df_type){
	case DF_Formation:
		if (arrangement.defender == MyNumber && Self.IsForward() && fieldinfo.IsOffside())
			return CP_fast_formation_speed;
		if (situation.gamestate != GS_Playing)
			return CP_slow_formation_speed;
		if (MyPlayer(arrangement.defender).attackness < 0.5f)
		{
			if ( fabs((Self.pos - arrangement.df_point).Angle()) > 150 && arrangement.df_point.x > -10)
			return CP_mid_formation_speed;
			else	
			return CP_fast_formation_speed;
		}
		else if (MyPlayer(arrangement.defender).attackness <= 0.7f)
			return CP_mid_formation_speed;
		else
		{
			if (fieldinfo.IsOffside())
				return CP_fast_formation_speed;
			else
				return CP_slow_formation_speed;
		}
		break;
	case DF_Block:
	case DF_Press:
		return CP_fast_formation_speed;
		break;
	case DF_Mark:
		if (arrangement.threat >= 0.85f){
			return CP_fast_formation_speed;
		}
		else {
			if (TheirPlayer(arrangement.defendee).balldist < 13.0f){
				return CP_fast_formation_speed;
			}
			return CP_mid_formation_speed;
		}
		break;
	}
		
	return CP_fast_formation_speed;
}

void Positioning::Position(){
	bool self_is_attack;
	if (situation.IsAttack){
		self_is_attack = true;
		if (!Self.IsForward() && fieldinfo.GetDefensiveSensitivity(situation.NextControlBallPos()) > 0.8f)
			self_is_attack = false;  
		if (Self.attackness <= 0.5f){ 
			if (fieldinfo.GetDefensiveSensitivity(situation.NextControlBallPos()) > 0.7f){
				self_is_attack = false;
			}
		}
	}
	else{
		if (motion.Num_MyVisiblePlayers() >= 1){
			if (motion.MyPlayer_Close2Ball(0).balldist < 1.5f
				&& Self.GetOffsensitivity() > 0.7f){
				self_is_attack = true;
			}
			else
				self_is_attack = false;
		}
		else
			self_is_attack = false;
	}

	if (self_is_attack){
		Offense();
	}
	else{
		Defense();
	}
}

void Positioning::CloseBlock(DF_Arrangement &arrangement){
	if (TheirPlayer(arrangement.defendee).distance > CP_block_dist)
		return;
	if (TheirPlayer(arrangement.defendee).ActiveRadius(situation.CurrentTime) > 0.6f)
		return;
	Vector Opos = TheirPlayer(arrangement.defendee).pos;
	if (Self.pos.x > Opos.x) return;
	Line opp_me;
	opp_me.LineFromTwoPoints(Self.pos,Opos);
	if (opp_me.dist(ball.pos) > 0.6f)
		return;
	Vector sectgoal = opp_me.intersection(fieldinfo.SideLines[SL_Left]);
	if (fabs(sectgoal.y) > SP_goal_width/2)
		return;
	DoLog(LOG_POSITIONING,"Close Block");
	arrangement.df_point = Self.pos + Polar2Vector(0.7f,(Opos - Self.pos).Angle());
}