pass.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 "pass.h"
#include "global.h"

/********************	Pass	*******************************/
Pass::Pass(){	
}

bool Pass::SmartPass(){
	
	ResetPassInfo();
	SetConsideredRoutes();
	SetConsideredPlayers();
	DoLog(LOG_PASS, "Considered players %d considered routes %d", num_players_considered,
		num_routes_considered);
	for(int i = 0; i < num_routes_considered; i ++){
		Explore(i);
	}
	DoLog(LOG_PASS, "numpassinfos %d", num_passinfo);
	return num_passinfo > 0;
	
}

KT_Res Pass::pass(PassInfo& passinf, Command& command){
	command.Reset();
	return kick.oakick(passinf.PassSpeed(), passinf.PassAngle(), command);
}

KT_Res Pass::pass(PassInfo& passinf){
	Command command;
	KT_Res res = pass(passinf, command);
	mediator.enroll(command, Action_pass, PriorityA);
	return res;
}

void Pass::AddPassInfo(UNum receiver, AngleDeg PS_angle, float PS_speed, bool passable){
	PassInfo inf;
	if (getpassinfo(receiver, PS_angle, PS_speed, inf)){
		if (get_pass_confidence(inf) > 0.5)
			AddPassInfo(inf, passable);
	}
}

void Pass::AddPassInfo(const PassInfo& passinf, bool passable){
	//if (passable){//passable means whether being confidential
	if(true){
		if(num_passinfo >= CP_max_passinfos) return;
		passinfo[num_passinfo++] = passinf;	
	}
	else{
		if(num_potential_passinfo >= CP_max_passinfos) return;
		potential_passinfo[num_potential_passinfo++] = passinf;
	}
}

void Pass::ResetPassInfo(){
	num_passinfo = 0;
	num_potential_passinfo = 0;
}

bool Pass::Is_Angle_Passable(AngleDeg angle){
	return sensory.DirConf(angle) > 0.77f && sensory.DirConf(angle + CP_passable_angle) > 0.77f
		&& sensory.DirConf(angle - CP_passable_angle) > 0.77f;
}

float Pass::PASSWeightByDistance(float distance){
	float weight = CP_passweight_conf_a * Sqr(distance) + CP_passweight_conf_b * distance + CP_passweight_conf_c;
	pose_limitation(weight, 0, 1);
	return weight;
}

float Pass::get_pass_confidence(PassInfo &inf){	
	return motion.calculatecontrol(inf.min_receiver_cycles, inf.min_opponent_cycles);
}


bool Pass::getpassinfo(UNum receiver, AngleDeg PS_angle, float PS_rapidness, PassInfo& passinfo){
	InterceptionInfo inf;
	float min_receiver_cycles = InfCycles, min_opponent_cycles = InfCycles, cycles;
	UNum most_danger_opponent = -1;
	Vector ballpos, ballvel, receive_ball_point;
	ballpos = ball.pos;
	ballvel = Polar2Vector(PS_rapidness, PS_angle);

	motion.getinterceptioninfo(MyPlayer(receiver).pos, ballpos, MyPlayer(receiver).global_vel, ballvel, MyPlayer(receiver).bodyfacing, inf);
	receive_ball_point = inf.IT_point;

	if (!fieldinfo.WithInField(receive_ball_point) || inf.IT_cycles >= InfCycles || inf.IT_cycles < 0)
		return false;

	min_receiver_cycles = inf.IT_cycles;

	int num = motion.Num_TheirVisiblePlayers();
	for (int i =0; i < num; i++){
		cycles = receive_ball_point.dist(TheirPlayer(motion.TheirVisiblePlayers[i]).pos) / 
			CP_assumed_interceptspeed;
		if (cycles < min_opponent_cycles){
			min_opponent_cycles = cycles;
			most_danger_opponent = motion.TheirVisiblePlayers[i];
		}
	}

	passinfo = PassInfo(receiver, PS_rapidness, PS_angle, min_receiver_cycles,
		min_opponent_cycles, most_danger_opponent, receive_ball_point);
	return true;
}


Line& Pass::Pline(int idx1, int idx2){
	if (!pline.IsDataKnown(idx1, idx2, situation.CurrentTime)){
		Line line;
		if (idx1 > idx2){
			Swap(idx1, idx2);
		}

		if (Players_considered[idx1] == MyNumber){
			line.LineFrompline(0, GetPlayer_considered(idx2).rel_pos_2_ball);
		}
		else{
			line.LineFrompline(GetPlayer_considered(idx1).rel_pos_2_ball, GetPlayer_considered(idx2).rel_pos_2_ball);
		}
		pline.SetData(line, idx1, idx2, situation.CurrentTime);
	}

	return pline.Data(idx1, idx2);
}

void Pass::SetConsideredPlayers(){
	num_players_considered = 0;
	Players_considered[num_players_considered ++] = MyNumber;

	int num = motion.Num_VisiblePlayers();
	
	for(int i = 0; i < num; i ++){
		if (num_players_considered >= CP_max_considerd_pass_players)
			break;
		Players_considered[num_players_considered ++] = motion.Players_Close2Ball[i];
	}
}

void Pass::SetConsideredRoutes(){
//set routes
	ResetRoutesBuffer();

	Ordered_Soft_Queue<float, SP_team_size> my_angles_queue;
	
	for(int No = 1; No <= SP_team_size; No ++){
		if (No != MyNumber && MyPlayer(No).pos_valid()){
			my_angles_queue.enqueue(MyPlayer(No).global_angle);
		}
	}

	int p = my_angles_queue.ActualHead();

	if (my_angles_queue.IsEmpty()) return;
	
	
	float ang, ang1, ang2, prev_ang = 360.0f;
	while(my_angles_queue.IsValid(p)){
		if (prev_ang == 360.0f){
			ang2 = my_angles_queue.GetData(my_angles_queue.CircularPrev(p));
		}
		else{
			ang2 = prev_ang;
		}

		ang1 = my_angles_queue.GetData(p);
		if (fabs(NormalizeAngle(prev_ang - ang1, 0.0f)) < 5.0){
			goto  GoNext;
		}

		prev_ang = ang1;
		
		if(!AddRoutes(ang1))
			break;

		ang = NormalizeAngle(ang2 - ang1, 0.0f);

		if (ang < 10.0f){
		//no route
		}
		else if (ang < 30.0f){
		//one route
			AddRoutes(NormalizeAngle(ang1 + ang /2.0f));
		}
		else if (ang < 60.0f){
			AddRoutes(NormalizeAngle(ang1 + ang / 3.0f));
			AddRoutes(NormalizeAngle(ang2 - ang / 3.0f));
		}
		else if (ang < 90.0f){
			AddRoutes(NormalizeAngle(ang1 + ang / 3.0f));
			AddRoutes(NormalizeAngle(ang1 + ang / 2.0f));
			AddRoutes(NormalizeAngle(ang2 - ang / 3.0f));
		}
		else if (ang < 120.0f){
			AddRoutes(NormalizeAngle(ang1 + ang / 6.0f));
			AddRoutes(NormalizeAngle(ang1 + ang / 3.0f));
			AddRoutes(NormalizeAngle(ang1 + ang / 2.0f));
			AddRoutes(NormalizeAngle(ang2 - ang / 3.0f));
			AddRoutes(NormalizeAngle(ang2 - ang / 6.0f));
		}
		else{
			AddRoutes(NormalizeAngle(ang1 + 15.0f));
			AddRoutes(NormalizeAngle(ang1 + 30.0f));
			AddRoutes(NormalizeAngle(ang1 + 60.0f));
			AddRoutes(NormalizeAngle(ang2 - 60.0f));
			AddRoutes(NormalizeAngle(ang2 - 30.0f));
			AddRoutes(NormalizeAngle(ang2 - 15.0f));
		}
GoNext:
		p = my_angles_queue.Next(p);
	}

	p = routes_queue.ActualHead();
	num_routes_considered = 0;
	while(routes_queue.IsValid(p)){
		if (num_routes_considered >= CP_max_considered_pass_routes)
			break;
		Routes_considered[num_routes_considered ++] = routes_queue.GetData(p);
		p = routes_queue.Next(p);
	}
}

bool Pass::AddRoutes(AngleDeg angle){
	if (routes_queue.NumDatas() >= CP_max_considered_pass_routes)
		return false;
	return routes_queue.enqueue(angle);
}

void Pass::ResetRoutesBuffer(){
	routes_queue.cleanup();
	num_routes_considered = 0;
}

Player& Pass::GetPlayer_considered(int idx){
#ifdef _DEBUG_MOD
	if (idx < 0 || idx >= num_players_considered){
		DoLog(LOG_PASS, "Bug here");
	}
#endif
	return GetPlayer(Players_considered[idx]);
}


int Pass::GetNearestExploredRoute(int routeidx){
	if (routeidx < 0 || routeidx >= num_routes_considered)
		return -1;
	int clockwise_idx, counter_idx;
	clockwise_idx = counter_idx = -1;
	for(int i = routeidx + 1; i < num_routes_considered; i ++){
		if (IsExplored(i)){
			clockwise_idx = i;
			break;
		}
	}

	for(i = routeidx -1; i >= 0; i --){
		if (IsExplored(i)){
			counter_idx = i;
			break;
		}
	}

	if (clockwise_idx == -1 && counter_idx == -1){
	//no route explored
		return -1;
	}

	if (clockwise_idx == -1){
		for(i = 0; i < counter_idx; i ++){
			if (IsExplored(i)){
				clockwise_idx = i;
				break;
			}
		}
		if (clockwise_idx == -1)