visualsystem.cpp

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

/********************************************************************************
 This module is designed to complete visual decision-making. Parts of confusing 
 hand codes, which assign priority, are removed.
*********************************************************************************/
/*
    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 "visualsystem.h"
#include "global.h"
#include "strategy.h"

/***********************   Visual System   *****************************************/
VisualSystem::VisualSystem(){
	b_validt = 0;
}

void VisualSystem::SetArounds(){
	int i, j;
	num_my_visible_players = 0;
	num_their_visible_players = 0;
	DoLog(LOG_SEE, "unk my %d", unknownmyplayerlist.numplayers);
	DoLog(LOG_SEE, "unk their %d", unknowntheirplayerlist.numplayers);
	DoLog(LOG_SEE, "teamless %d", teamlessplayerlist.numplayers);
/******   Get visible players list  ********************/
	for(i = 1; i <= SP_team_size; i++){
		if (i != MyNumber && MyPlayer(i).pos_valid()){
			MyVisiblePlayers[num_my_visible_players++] = i;
			DoLog(LOG_SEE,"My %d(%.2f,%.2f)",i,MyPlayer(i).pos.x,MyPlayer(i).pos.y);
		}
		if (TheirPlayer(i).pos_valid() || TheirPlayer(i).Is_goalie){
			TheirVisiblePlayers[num_their_visible_players++] = i + SP_team_size;
			DoLog(LOG_SEE,"Opp %d(%.2f,%.2f)",i,TheirPlayer(i).pos.x,TheirPlayer(i).pos.y);
			if(TheirPlayer(i).Is_goalie){
				DoLog("their goalie %.2f %.2f posconf %.2f", TheirPlayer(i).pos.x, TheirPlayer(i).pos.y, TheirPlayer(i).pos_conf);
			}
		}
	}
	
	for(i = 0; i < unknownmyplayerlist.numplayers; i ++){
		MyVisiblePlayers[num_my_visible_players++] = 2 * SP_team_size + i +1;
		DoLog(LOG_SEE,"My unk(%.2f,%.2f)",MyPlayer(2 * SP_team_size + i +1).pos.x,MyPlayer(2 * SP_team_size + i +1).pos.y);
		if (num_my_visible_players >= CP_max_visible_players)
			break;
	}
	for(i = 0; i < unknowntheirplayerlist.numplayers; i ++){
		TheirVisiblePlayers[num_their_visible_players++] = 4 * SP_team_size + i + 1;
		DoLog(LOG_SEE,"Their unk(%.2f,%.2f)",TheirPlayer(4* SP_team_size + i +1).pos.x,TheirPlayer(4 * SP_team_size + i +1).pos.y);
		if (num_their_visible_players >= CP_max_visible_players)
			break;
	}

	for(i = 0; i < teamlessplayerlist.numplayers; i ++){
		TheirVisiblePlayers[num_their_visible_players++] = 6 * SP_team_size + i + 1;
		DoLog(LOG_SEE,"Their teamless(%.2f,%.2f)",TheirPlayer(6* SP_team_size + i +1).pos.x,TheirPlayer(6 * SP_team_size + i +1).pos.y);
		if (num_their_visible_players >= CP_max_visible_players)
			break;
	}

	for(i = 0; i < num_my_visible_players; i++){
		MyPlayers_Close2Ball[i] = MyPlayers_Close2Me[i] = MyPlayers_InFront[i] = MyVisiblePlayers[i];
	}
	for(i = 0; i < num_my_visible_players - 1; i++){
		for ( j = i + 1; j < num_my_visible_players; j ++){
			if (MyPlayer(MyPlayers_Close2Ball[j]).balldist < MyPlayer(MyPlayers_Close2Ball[i]).balldist){
				Swap(MyPlayers_Close2Ball[i], MyPlayers_Close2Ball[j]);
			}
			if (MyPlayer(MyPlayers_Close2Me[j]).distance < MyPlayer(MyPlayers_Close2Me[i]).distance){
				Swap(MyPlayers_Close2Me[i], MyPlayers_Close2Me[j]);
			}
			if (MyPlayer(MyPlayers_InFront[j]).pos.x > MyPlayer(MyPlayers_InFront[i]).pos.x){
				Swap(MyPlayers_InFront[j], MyPlayers_InFront[i]);
			}
		}
	}

	for(i = 0; i < num_their_visible_players; i++){
		TheirPlayers_Danger[i] = TheirPlayers_Close2Ball[i] = TheirPlayers_Close2Me[i] = TheirPlayers_InFront[i] = TheirVisiblePlayers[i];
	}
	for(i = 0; i < num_their_visible_players - 1; i++){
		for ( j = i + 1; j < num_their_visible_players; j ++){
			if (TheirPlayer(TheirPlayers_Close2Ball[j]).balldist < TheirPlayer(TheirPlayers_Close2Ball[i]).balldist){
				Swap(TheirPlayers_Close2Ball[i], TheirPlayers_Close2Ball[j]);
			}
			if (TheirPlayer(TheirPlayers_Close2Me[j]).distance < TheirPlayer(TheirPlayers_Close2Me[i]).distance){
				Swap(TheirPlayers_Close2Me[i], TheirPlayers_Close2Me[j]);
			}
			if (TheirPlayer(TheirPlayers_InFront[j]).pos.x > TheirPlayer(TheirPlayers_InFront[i]).pos.x){
				Swap(TheirPlayers_InFront[j], TheirPlayers_InFront[i]);
			}
			if (TheirPlayer(TheirPlayers_Danger[j]).GetDefsensitivity()
				> TheirPlayer(TheirPlayers_Danger[i]).GetDefsensitivity())
				Swap(TheirPlayers_Danger[i],TheirPlayers_Danger[j]);
		}
	}

	for(i = 0; i < num_my_visible_players; i ++)
		MyPlayers_InBehind[i] = MyPlayers_InFront[num_my_visible_players - 1 - i];


	for(i = 0; i < num_their_visible_players; i++)
		TheirPlayers_InBehind[i] = TheirPlayers_InFront[num_their_visible_players - 1 - i];

	num_visible_players = num_my_visible_players + num_their_visible_players;

	i = 0; j = 0; int num_players_close2me = 0;
	while(i < num_my_visible_players || j < num_their_visible_players){
		if (i >= num_my_visible_players){
			while(j < num_their_visible_players){
				Players_Close2Me[num_players_close2me++] = TheirPlayers_Close2Me[j++];
			}
		}
		else if(j >= num_their_visible_players){
			while(i < num_my_visible_players){
				Players_Close2Me[num_players_close2me++] = MyPlayers_Close2Me[i++];
			}
		}
		else{
			if (MyPlayer_Close2Me(i).distance < TheirPlayer_Close2Me(j).distance)
				Players_Close2Me[num_players_close2me++] = MyPlayers_Close2Me[i++];
			else
				Players_Close2Me[num_players_close2me++] = TheirPlayers_Close2Me[j++];
		}
	}

	i = 0; j = 0; int num_players_close2ball = 0;
	while(i < num_my_visible_players || j < num_their_visible_players){
		if (i >= num_my_visible_players){
			while(j < num_their_visible_players){
				Players_Close2Ball[num_players_close2ball++] = TheirPlayers_Close2Ball[j++];
			}
		}
		else if(j >= num_their_visible_players){
			while(i < num_my_visible_players){
				Players_Close2Ball[num_players_close2ball++] = MyPlayers_Close2Ball[i++];
			}
		}
		else{
			if (MyPlayer_Close2Ball(i).balldist < TheirPlayer_Close2Ball(j).balldist)
				Players_Close2Ball[num_players_close2ball++] = MyPlayers_Close2Ball[i++];
			else
				Players_Close2Ball[num_players_close2ball++] = TheirPlayers_Close2Ball[j++];
		}
	}

	SetBallInf();
	DoLog(LOG_SEE, "setarounds complete");
}

/*find out who is closes to the ball */
void VisualSystem::SetBallInf(){
	UNum closestp = -1;
	float min_dist = 100.0f;
	if(num_my_visible_players > 0){
		if(MyPlayers_Close2Ball[0] == MyNumber && num_my_visible_players > 1){
			closestp = MyPlayers_Close2Ball[1];
		}
		else{
			closestp = MyPlayers_Close2Ball[0] ;
		}
		min_dist = MyPlayer(closestp).balldist;
		ball.Setball_CPlayer(closestp,MyPlayer(closestp).pos,situation.CurrentTime);
	}
	if(num_their_visible_players > 0){
		if(TheirPlayer(TheirPlayers_Close2Ball[0]).balldist < min_dist){
			closestp = TheirPlayers_Close2Ball[0] ;
			ball.Setball_CPlayer(closestp,TheirPlayer(closestp).pos,situation.CurrentTime);
		}
	}	
}

Player& VisualSystem::MyPlayer_Close2Me(int idx){
	return MyPlayer(MyPlayers_Close2Me[idx]);
}

Player& VisualSystem::TheirPlayer_Close2Me(int idx){
	return TheirPlayer(TheirPlayers_Close2Me[idx]);
}

Player& VisualSystem::GetPlayer_Close2Me(int idx){
	return GetPlayer(Players_Close2Me[idx]);
}

Player& VisualSystem::MyPlayer_Close2Ball(int idx){
	return MyPlayer(MyPlayers_Close2Ball[idx]);
}

Player& VisualSystem::TheirPlayer_Close2Ball(int idx){
	return TheirPlayer(TheirPlayers_Close2Ball[idx]);
}

Player& VisualSystem::GetPlayer_Close2Ball(int idx){
	return GetPlayer(Players_Close2Ball[idx]);
}

Player& VisualSystem::MyPlayer_InFront(int idx){
	return MyPlayer(MyPlayers_InFront[idx]);
}

Player& VisualSystem::MyPlayer_InBehind(int idx){
	return MyPlayer(MyPlayers_InBehind[idx]);
}

Player& VisualSystem::TheirPlayer_InFront(int idx){
	return TheirPlayer(TheirPlayers_InFront[idx]);
}

Player& VisualSystem::TheirPlayer_InBehind(int idx){
	return TheirPlayer(TheirPlayers_InBehind[idx]);
}

void VisualSystem::Raise(VisualObjType otype, AngleDeg angle, float priority){
	mediator.enroll(priority, angle);
	DoLog(LOG_VDEC, "R %.0f, %.3f", angle, priority);
}

void VisualSystem::Raise(VisualObjType otype, AngleDeg angle, AngleDeg distribute, float priority){
	Raise(otype, angle, 0.7f * priority);
	Raise(otype, angle - distribute, 0.4f * priority);
	Raise(otype, angle + distribute, 0.4f * priority);
}

/*raise a visual request for observing player*/
void VisualSystem::Raiseplayer(Player& p, float priority){
	float radius = p.ActiveRadius(situation.CurrentTime);
	float dist = Max(p.distance + 0.001f, radius);
	float spreadangle = ASin(radius / dist);

	if(p.Update_seen_time() != p.original_postime){//hearpos isn't accurate enough.
		spreadangle += 10.0f;
	} 
	if(p.Isforgot()){
		DoLog(LOG_VDEC, "find %d %.2f", p.InsideNO, priority);
		Findforgetobj(VO_player, p.global_angle, spreadangle, priority);
	}
	else
		Raise(VO_player, p.global_angle, spreadangle, priority);
}

/*raise a visual request for finding */
void VisualSystem::Findforgetobj(VisualObjType otype, AngleDeg angle, AngleDeg distribute, float maxpriority){
	AngleDeg tangle = angle - distribute;
	float priority;
	for(int i = 0; i < 4; i++){
		float conf = sensory.DirConf(tangle);
		if(conf < 0.9f && !Predictseen(tangle)){
			priority = Min(4.0f * (1 - conf), 1.0f);
			Raise(otype, tangle, maxpriority * priority * 0.5f);
		}
		tangle += distribute * 0.666f;
	}	
}

void VisualSystem::GetActionInf(){
	mediator.Getbestaction(bestaction);
	nextselfpos = Self.pos + Self.global_vel;
	if(bestaction.GetCMDType() == CMD_dash){
		nextselfpos += Polar2Vector(bestaction.GetPower() * action.Dashrate(), Self.bodyfacing);
	}
	DoLog(LOG_VDEC,"next self pos(%.2f, %.2f)", nextselfpos.x, nextselfpos.y);
}

void VisualSystem::DoVisualDecision(){
	PredicInfComing();
	Updatebvalidt();

	GetActionInf();
	
	if(FindBall())	return;

	float ball_priority = 0;
	float offlook_priority = 0;
	float deflook_priority = 0;
	float offside_priority = 0;
	float extendit_priority = 0;

//you should assign priority here.	

	bool canfeelball = Isballbyfeel();
	if(!canfeelball)
		AssureBall(ball_priority);

	DoLog(LOG_VDEC,"off %.2f, def %.2f", offlook_priority, deflook_priority);
	DoPassLook(offlook_priority);
	DoExtendITLook(extendit_priority);
	DoDefendLook(deflook_priority);	
	DoOffsideLook(offside_priority);

	DoViewModeDecision(canfeelball);
}

void VisualSystem::DoOffsideLook(float maxpriority){
	if(maxpriority <=0 || Self.attackness < 0.7f) return;//Self.pos.x <= 0 || Self.pos.x < ball.pos.x || 

	if(fieldinfo.IsOffside() || !fieldinfo.Maybeoffside()) return;
	UNum opp = fieldinfo.Susoffsideopp.ChkData(situation.CurrentTime);
	DoLog(LOG_VDEC, "suspicious offside %d", opp);
	Raiseplayer(TheirPlayer(opp), maxpriority * 0.5f);
	return;
}

bool VisualSystem::FindBall(){
/*	When Ball is missing, look for it	*/
	float CP_valid_dirconf = 0.77f;
	if(b_validt <= 0 && ball.pos_conf < CP_valid_dirconf){
		DoLog(LOG_VDEC, "find ball");
		if((Predictconf(ball.global_angle) < 0.8f || (!ball.Isforgot() && ball.pos_conf > 0.5f)) && !situation.ClockStopped){
			//ball maybe still in course
			float distribute = ATan2(1.5f * (situation.CurrentTime - ball.original_postime), ball.distance);
			distribute = Max(distribute, 20.0f);
			Raise(VO_ball, ball.global_angle, 2 * distribute,1.0f);
		}else{
			DoSeek(1.0f, ball.global_angle);
		}
		return true;
	}
 	return false;
}