visualsystem.cpp

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

/*****predict next visual inf within current view angle	*********/
/*****when clockstopped, prediction is invalid now		*********/
void VisualSystem::PredicInfComing(){
	Vinfo_coming = false;
	if(situation.SightTime != situation.CurrentTime && !situation.ClockStopped){
		if(Self.viewwidth.Data(situation.CurrentTime) == VW_Narrow
			|| (situation.SightTime == situation.CurrentTime - 2 && Self.viewwidth.Data(situation.CurrentTime) == VW_Normal)
			|| situation.SightTime < situation.CurrentTime - 2){
			Vinfo_coming = true;
			Vcominginfo_angle = Self.headfacing;
			Vcominginfo_width = sensory.MyViewWidth(Self.viewwidth.Data(situation.CurrentTime));
			DoLog(LOG_VDEC, "coming visual info (%.0f %.0f)" , NormalizeAngle(Vcominginfo_angle - Vcominginfo_width/2),
				NormalizeAngle(Vcominginfo_angle + Vcominginfo_width/2));
		}
	}
}

void VisualSystem::Updatebvalidt(){
	ITconfirm = false;
	if(b_validt <= 0){
		if(!ball.newpos){
			b_validt --;
		}else if(!ball.newvel){
			b_validt = ball.original_postime - situation.CurrentTime;
		}else{
			UpdateMinITcycles();
			b_validt = DirValidateball(lastITcycles);
		}
	}else{		
		UpdateMinITcycles();
		b_validt = DirValidateball(lastITcycles);
	}
	if(situation.ClockStopped) b_validt = Max(b_validt, 0); 
	DoLog(LOG_VDEC, "valid time %d", b_validt);
	if(num_visible_players > 0 && b_validt > 0){
		if(GetPlayer_Close2Ball(0).balldist - 0.5f * Max(1.0f, ball.posgap.ChkData(situation.CurrentTime).mod()) < SP_kickable_area){
			DoLog(LOG_VDEC, "modify valid t %d %.2f %.2f", GetPlayer_Close2Ball(0).InsideNO, GetPlayer_Close2Ball(0).balldist, ball.posgap.ChkData(situation.CurrentTime).mod());
			b_validt = 0;
		}
	}
} 

void VisualSystem::UpdateMinITcycles(){
	if(!situation.BallFree){
		lastITplayer = situation.ballcontroller;
		lastITcycles = 0;
	}else{
		//continue record it player
		if(!ball.newpos && int(situation.min_IT_cycles) >= lastITcycles &&
			lastITplayer > 0 && lastITplayer < 23 && !GetPlayer(lastITplayer).newpos){
			lastITcycles --;
			DoLog(LOG_VDEC, "special deal");
		}else{
			lastITplayer = situation.most_promising_controller;
			lastITcycles = (int)situation.min_IT_cycles;
		}
	}
	DoLog(LOG_VDEC,"IT player%d cycles %d", lastITplayer, lastITcycles);
}

int VisualSystem::DirValidateball(int maxcycles){
	maxcycles = Min(maxcycles, CP_max_bvalidcycles);
	if(maxcycles <= 0) return maxcycles;
	
	float CP_valid_dirconf = 0.81f;
	Vector pos[CP_max_bvalidcycles];
	ball.PredictPos(pos, maxcycles); 
	float leftang = ball.global_angle;
	float rightang = leftang;
	Vector relpos;
	float tmpang1, tmpang2, radius, dist;
	for(int i = 0; i < maxcycles; i++){
		radius = i + 1.0f;
		relpos = pos[i] - Self.pos;
		dist = relpos.mod() + CP_max_bvalidcycles;
		tmpang1 = relpos.Angle();
		tmpang2 = ASin(radius / dist);
		if(leftang > tmpang1 - tmpang2){
			leftang = tmpang1 - tmpang2;
			if(sensory.DirConf(leftang) < CP_valid_dirconf){
				ITconfirm = true;
				ITconfirmagl = NormalizeAngle(leftang - 10);
				return i+1;
			}
		}
		if(rightang < tmpang1 + tmpang2){
			rightang = tmpang1 + tmpang2;
			if(sensory.DirConf(leftang) < CP_valid_dirconf){
				ITconfirm = true;
				ITconfirmagl = NormalizeAngle(rightang + 10); 
				return i+1;
			}
		}
	}
	return maxcycles;
}

float VisualSystem::GetPromisingAngle(VisualObjType obj, UNum idx){
	MobileObject* mobj = NULL;
	if(obj == VO_ball){
		mobj = &ball;
	}else if(obj == VO_player){
		mobj = &GetPlayer(idx);
	}
	if(mobj != NULL){
		if(mobj->pos_conf > CP_min_valid_conf){
			return mobj->global_angle; 
		}else{//
		}
	}else{
		//shouldn't be here;
	}
	return 0;
}

void VisualSystem::DoSeek(float maxpriority, AngleDeg recommendedangle){
	int CP_seek_division = 12;
	for(int i = 0; i < CP_seek_division; i++){
		float angle = recommendedangle + 360.0f/CP_seek_division * i;
		float conf = Predictconf(angle);
		if(conf > 0.9f) continue;
		float priority = 0.3f + 0.7f * (1 - conf);
		priority += 0.1f - 0.2f * (float)fabs(NormalizeAngle(360.0f/CP_seek_division * i)) / 180.0f; 
		Raise(VO_route, angle, priority * maxpriority);
	}
}

void VisualSystem::DoExtendITLook(float maxpriority){
	if(!ITconfirm || maxpriority <= 0 || Predictseen(ITconfirmagl)) return;

	maxpriority *= 0.25f;
	Raise(VO_route, ITconfirmagl, maxpriority);
	DoLog(LOG_VDEC, "extend it %.2f priority %.2f", ITconfirmagl, maxpriority);
}

float VisualSystem::Predictconf(float ang){
	if(Predictseen(ang))
		return 1.0f;
	else
		return sensory.DirConf(ang);
}

bool VisualSystem::Predictseen(float ang){
	if(Vinfo_coming && fabs(NormalizeAngle(ang - Vcominginfo_angle)) < Vcominginfo_width)
		return true;
	else
		return false;
}

void VisualSystem::AssureBall(float priority){	
	Vector relpos;
	float ballangle, distribute_angle, radius, dist;
	if(!situation.BallFree){
		relpos = ball.Pos(ball.original_postime) - nextselfpos;
		ballangle = relpos.Angle();
		dist = relpos.mod() + 0.001f;
		radius = (situation.CurrentTime - ball.original_postime + 1) * SP_ball_speed_max; 
		radius = Min(radius, dist);
		distribute_angle = ASin(radius / dist);
	}else if(b_validt > 0){
		ballangle = (ball.PredictPos(1) - nextselfpos).Angle();
		distribute_angle = 5;
	}else{
		ballangle = ball.global_angle;
		distribute_angle = 15;
	}

	
	if(!ball.Isforgot()){
		Raise(VO_ball, ballangle, distribute_angle, priority);
	}else{
		DoLog(LOG_VDEC, "ball is forgot");
		if(ball.Rs_valid())
			Findforgetobj(VO_ball, ballangle, distribute_angle, priority);
		else
			FindBall();
	}	
	
}


void VisualSystem::DoPassLook(float maxpriority){
	if(maxpriority <= 0) return;
	float priority;
	int i;

	for(i = 1; i <= SP_team_size; i++){
		if(MyPlayer(i).original_postime == 0) continue;//doesn't exit player

		priority = LookTeammatePriority(MyPlayer(i));
		if(priority <= 0.0001f) continue;
		Raiseplayer(MyPlayer(i), maxpriority * priority);
	}

	for(i = 1; i <= SP_team_size; i++){		
		if(TheirPlayer(i).original_postime == 0) continue;

		priority = LookOppPriority(TheirPlayer(i));
		if(priority <= 0.0001f) continue;
		Raiseplayer(TheirPlayer(i), maxpriority * priority * 0.7f);
	}
	//for shoot
	if((ball.kickable() || bestaction.GetActionType() == Action_interception) && fieldinfo.WithInShootableArea(Self.pos) ){
		//Rect shoot_area(SP_semi_pitch_length, 30, SP_penalty_area_width /2, - SP_penalty_area_width /2);
		bool maybe_shoot;
		if(bestaction.GetActionType() == Action_interception){
			maybe_shoot = bool(motion.GuessShoot(Self.IT_inf.IT_point, (int)Self.IT_inf.IT_cycles) > 0);
		}else if(bestaction.GetActionType() ==  Action_shoot){
			maybe_shoot = true;
		}else{
			maybe_shoot = bool(motion.GuessShoot(ball.pos + Vector(1,3), 0) >0 || motion.GuessShoot(ball.pos + Vector(1, -3), 0) >0);
		}

		if(maybe_shoot){
			//pay some more attention to goalie
		}else{
			DoLog(LOG_VDEC, "don't intend to shoot");
		}
	}
	//for pass
	if(motion.WaitPassLook()){
		DoLog(LOG_VDEC, "look at pass receiver %.2f", maxpriority * 2.0f);
		Raiseplayer(MyPlayer(motion.GetReceiver()), maxpriority * 2.0f);
		Raise(VO_route, motion.GetPassAngle(), maxpriority * 0.3f);
	}
}

float VisualSystem::LookTeammatePriority(Player& player){
	if(player.distance > CP_passable_distance) return 0.0f;

	return 0.0f;
}

float VisualSystem::LookOppPriority(Player& player){
	if(player.distance > CP_passable_distance) return 0.0f;
	
	return 0.0f;
}

float VisualSystem::DefLookOppPry(Player& player){
	return 0.0f;
}

float VisualSystem::GetPriority(float concern, float conf){
	if(concern < 0.1) return 0;
	return float(concern * sqrt(conf));
}

void VisualSystem::DoDefendLook(float maxpriority){
	if(maxpriority <= 0) return;
	for(int i = 1; i <= SP_team_size; i++){
		if(TheirPlayer(i).distance > CP_def_concern_area) continue;
		float priority = DefLookOppPry(TheirPlayer(i));
		if(priority < 0.001f) continue;
		Raiseplayer(TheirPlayer(i), maxpriority * priority / 2);
	}
}

void VisualSystem::DoGoalieLook(float maxpriority){
	float concern, conf, priority;
	for(int i = 1; i<= SP_team_size; i++){
		if(TheirPlayer(i).original_postime == 0) continue;

		if(TheirPlayer(i).GetDefsensitivity() < 0.6f) continue;
		
		concern = Sqr(TheirPlayer(i).GetDefsensitivity());
		if(!Isobjconfmax(TheirPlayer(i))){
			conf = (1 - TheirPlayer(i).pos_conf) / (1 - CP_min_valid_conf + 0.2f);
			conf = Min(conf, 1.0f);
			priority = GetPriority(concern, conf);
		}else
			priority = concern * 0.1f;

		if(priority <= 0.0001f) continue;
		DoLog(LOG_VDEC,"opp%d, concern%.2f, conf%.2f p%.2f", i, concern, conf, priority);
		Raise(VO_player, TheirPlayer(i).global_angle, priority * maxpriority);
	}
} 

bool VisualSystem::Isballbyfeel(){
	if(ball.distance > SP_feel_distance + SP_ball_speed_max) return false;

	if(ball.kickable())	return true;

	Vector ballgap = ball.posgap.ChkData(situation.CurrentTime) + ball.velgap.ChkData(situation.CurrentTime);
	if((ball.PredictPos(1) - nextselfpos).mod() < SP_feel_distance - ballgap.mod() && b_validt > 0)
		return true;
	else
		return false;
}

bool VisualSystem::Isobjconfmax(MobileObject& obj){
	if((situation.CurrentTime - obj.original_postime < 2 && obj.Rs_valid())
		|| (Predictseen(obj.global_angle) && !obj.Isforgot()))
		return true;
	else
		return false;
}

void VisualSystem::DoViewModeDecision(bool forcenarrow){
	if(Self.Is_goalie && situation.IsGoalieKick){
		mediator.SetViewWidth(VW_Wide);
		return;
	} 
	if(ball.distance > 50.0f){
		mediator.SetViewWidth(VW_Wide);
		return;
	}else if(ball.distance > 30.0f){
		mediator.SetViewWidth(VW_Normal);
		return;
	}
	
	if(Self.viewwidth.Data(situation.CurrentTime) == VW_Normal){
		float prevtimegap = watch.TimeElapsedSight(); 
		if(prevtimegap > SP_send_step * 0.5f){
			mediator.SetViewWidth(VW_Normal);
			DoLog(LOG_VDEC, "keep on visual mode %.2f", prevtimegap);
			return;
		}
	}

	if(forcenarrow){
		mediator.SetViewWidth(VW_Narrow);
		return;
	}
	if(sensory.LastSightTime != situation.CurrentTime && Self.viewwidth.Data(situation.CurrentTime) == VW_Narrow){
		if(!ball.Isforgot() && fabs(NormalizeAngle(ball.global_angle - Self.headfacing)) < sensory.MyViewWidth(VW_Narrow)){
			mediator.SetViewWidth(VW_Narrow);
			DoLog(LOG_VDEC, "keep on visual mode");
		}
	}
}