objects.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 "stdafx.h"

#include "Objects.h"
#include "global.h"

/*******************************   Object   **************************/
Object::Object(){
	pos_conf = 0;
	pos_time = -1;
	coordinate_time = -1;
	global_angle = 0;
}


void Object::set_polar(AngleDeg ang,Time time){
	//do not exchange the order of the next two statements
	seenposinfo.Setdata(Vector(seenposinfo.Data(seentime).x, ang), time);
	seentime = time;
	seen = true;
}

void Object::set_polar(AngleDeg ang,float d,Time time){
	seenposinfo.Setdata(Vector(d, ang), time);
	seentime = time;
	sdtime = time;
	seen = true;
}

void Object::set_pos(float X,float Y,bool rotate){
	if (!rotate)
		pos = Vector(X,Y);
	else
		pos = Vector(-X,-Y);
}

void Object::set_global_angle(AngleDeg ang,bool rotate){
	if(!rotate)
		global_angle = ang;
	else
		global_angle = NormalizeAngle(180 + ang);
}

void Object::set_conf_param(float max,float decay,float min){
	max_conf = max;
	conf_decay = decay;
	min_valid_conf = min;
}

void Object::set_objtype(ObjType objtype){
	this->objtype = objtype;
}

bool Object::pos_valid(){
	return bool(pos_conf >= min_valid_conf);
}
/***************************   Mobile Object       *********************/
MobileObject::MobileObject(){
	Object::Object();
	speed_conf = 0;
	speed_conf = 0;
	global_vel = 0;
	update_seen_time = -1;
	seentime = -1;
	sdtime = -1;
}

void MobileObject::set_chinfo(float distChng,float dirChng,Time time){
	seenchinfo.Setdata(Vector(distChng, dirChng), time);
	seenchtime = time;
	seenmoving = true;
}

Vector MobileObject::Pos(Time time){
	return gpos.IsDataKnown(time) ? gpos.Data(time) : pos;
}

Vector MobileObject::Vel(Time time){
	return gvel.IsDataKnown(time) ? gvel.Data(time) : global_vel;
}

float MobileObject::ActiveRadius(Time time){
	return rsradius.ChkData(time);
}

void MobileObject::update_end(){
	if(sensory.NewSight && !seen && inview(sensory.LastSightTime) && distance < SP_team_far_length){
		DoLog(LOG_UPDATE, "forget obj (%.2f %.2f)", pos.x, pos.y);
		forget();
	}

	pos = Pos(situation.CurrentTime);
	global_vel = Vel(situation.CurrentTime);
	rel_pos = pos - Self.pos;
	rel_pos_2_bodyfacing = rel_pos.Rotate(- Self.bodyfacing);
	rel_pos_2_headfacing = rel_pos.Rotate(- Self.headfacing);
	global_angle = rel_pos.Angle();
	distance = rel_pos.mod();
	rel_angle_2_bodyfacing = NormalizeAngle(global_angle - Self.bodyfacing);
	rel_angle_2_headfacing = NormalizeAngle(global_angle - Self.headfacing);

	speed = global_vel.mod();
	vel_angle = global_vel.Angle();
	rel_vel = global_vel.Rotate(- Self.bodyfacing);
	seen = false;
	seenmoving = false;
	heardupdate = false;
}

void MobileObject::update(Time time){
	newpos = false;
	newvel = false;

	update_seen(time);
	update_heard(time);
	
	estimate(time);
	pos = Pos(sensory.LastSightTime);
	rel_pos = pos - Self.Pos(sensory.LastSightTime); 
	distance = rel_pos.mod();
}

void MobileObject::update_seen(Time time){
	if (!sensory.NewSight) return;
	/*       Position         */
	if(seen){
		Time last_update_seen_time = Max(update_seen_time, time - 2);
		Vector pos, rel_vel, rpos;
		for(int t = last_update_seen_time + 1; t <= time; t ++){
			if (seenposinfo.IsDataKnown(t)){
				rpos = Polar2Vector(seen_distance(t), seen_rel_angle(t)); 
				pos = rpos.Rotate(Self.Headfacing(t)) + Self.Pos(t);
				estimate(t -1);
				gpos.Setdata(pos, t);
				pos_time = t;
				coordinate_time = t;
				pos_conf = max_conf;
				newpos = true;
				original_postime = t;
				if (objtype == OBJ_Ball)
					DoLog(LOG_UPDATE,"See Ball(%.2f %.2f) at %d", Pos(t).x, Pos(t).y, t);
				update_seen_time = t;
				
				/*****************/
				rspos.Setdata(pos, t);
				rsradius.Setdata(0,t);
				rspos_time = t;
				rs_valid = true;
				forgot = false;
				/*       Velocity         */
				if (seenchinfo.IsDataKnown(t)){
					rpos.Normalize();
					rel_vel.x = seen_distCh(t) * rpos.x - (seen_dirCh(t) * PI / 180.0f * seen_distance(t) * rpos.y);
					rel_vel.y = seen_distCh(t) * rpos.y + (seen_dirCh(t) * PI / 180.0f * seen_distance(t) * rpos.x);

					gvel.Setdata(Self.Vel(t) + rel_vel.Rotate(Self.Headfacing(t)), t);
					vel_time = t;
					speed_conf = max_conf;
					if (objtype == OBJ_Ball)
						DoLog(LOG_UPDATE, "See Ball Moving with (%.2f %.2f) at %d", Vel(t).x, Vel(t).y, t);
					
					newvel = true;
					original_veltime = t;
				}
			}
		}
	}
}

void MobileObject::estimate(Time time){
	Time t;
	if(vel_time < pos_time - 5){//for unknown player, maybe no old vel;
		vel_time = pos_time;
		gvel.Setdata(0, vel_time);
		speed_conf = 0;
	}
	for(t = vel_time; t < pos_time; t ++){
		estimate_vel(t + 1);
	}
	if(sensory.LastSightTime > coordinate_time && coordinate_time > 0 && gpos.IsDataKnown(coordinate_time)){
		float posconfbk = pos_conf;
		for(t = coordinate_time; t < pos_time; t++){			
			estimate_pos(t + 1);
		}
		pos_conf = posconfbk;
	}
	for(t = pos_time; t < time; t ++){
		estimate_vel(t + 1);
		estimate_pos(t + 1);
	}
}

void MobileObject::estimate_vel(Time time){
	Vector evel = gvel.ChkData(time -1);

	if (objtype == OBJ_Ball)
		evel += action.KickEffect(time -1);
	if (evel.mod() > max_speed){
		evel = evel / evel.mod() * max_speed;
	}
	evel *= speed_decay;
	gvel.Setdata(evel, time);
	speed_conf *= conf_decay;
	vel_time = time;
	if (objtype == OBJ_Ball){
		DoLog(LOG_UPDATE, "Estimate Ball Moving with (%.2f %.2f) at %d", Vel(time).x, Vel(time).y, time);
	}
}

void MobileObject::estimate_pos(Time time){
	gpos.Setdata(Pos(time -1) + gvel.ChkData(time) / speed_decay + Self.pos_gap.ChkData(time) , time);
	pos_conf *= conf_decay;
	pos_time = time;
	if(Self.pos_gap.IsDataKnown(time)){
		coordinate_time = time;
		if (objtype == OBJ_Ball){
			DoLog(LOG_UPDATE,"coordinate time %d", time);
		}
	}
	if (objtype == OBJ_Ball){
		DoLog(LOG_UPDATE, "Estimate Ball (%.2f %.2f) at %d", Pos(time).x, Pos(time).y, time);
	}
}

bool MobileObject::Heardupdate(){
	return heard;
}

void MobileObject::update_heard(Time time){
	if (Heardupdate()){
		gpos.Setdata(heardposinfo.data, heardposinfo.time);
		pos_time = heardposinfo.time;
		coordinate_time = pos_time;
		pos_conf = Exp(conf_decay, float(pos_time - heard_org_postime)) * max_conf;
		original_postime = heard_org_postime;
		
		newpos = true;
		heardupdate = true;
		
		rspos_time = heardposinfo.time;
		rspos.Setdata(heardposinfo.data, rspos_time);
		rsradius.Setdata((rspos_time - heard_org_postime) * max_speed, rspos_time);
		rs_valid = true;
		forgot = false;
		
		if(objtype == OBJ_Ball){
			DoLog(LOG_HEAR, "ball pos is heard at %d (%.2f %.2f)", original_postime, heardposinfo.data.x, heardposinfo.data.y);
			//take care, here we set player's iscontrolball
			for(int No = 1; No <= SP_team_size; No++){
				MyPlayer(No).Set_Iscontrolball_Fromheard(heardposinfo.time);
				TheirPlayer(No).Set_Iscontrolball_Fromheard(heardposinfo.time);
				unknowntheirplayerlist.Set_IsControlball_FromHeard(heardposinfo.time);
				unknownmyplayerlist.Set_IsControlball_FromHeard(heardposinfo.time);
				teamlessplayerlist.Set_IsControlball_FromHeard(heardposinfo.time);
			}
		}
		
		if (heard_org_veltime > original_veltime){
			gvel.Setdata(heardvelinfo.data, heardvelinfo.time);
			vel_time = heardvelinfo.time;
			speed_conf = Exp(conf_decay, float(vel_time - heard_org_veltime)) *  max_conf;
			original_veltime = heard_org_veltime;
			
			newvel = true;
			if(objtype == OBJ_Ball){
				DoLog(LOG_HEAR, "ball vel is heard at %d (%.2f %.2f)", original_veltime, heardvelinfo.data.x, heardvelinfo.data.y);
			}
		}
	}
	heard = false;
}

bool MobileObject::inview(Time time){
	if (!pos_valid()) return false;

	if (pos_time < time){
		estimate(time);
	}
	Vector tmp_pos = (Pos(time) - Self.Pos(time)).Rotate(-Self.Headfacing(time));
	if(tmp_pos.mod() < SP_feel_distance * 0.5f) return true; 
	//DoLog(7,"The Object is assumed to be at rel(%.2f %.2f)", tmp_pos.x, tmp_pos.y);
	float slide_dist = 1.0f / Sin(sensory.MyViewAngle(Self.viewwidth.Data(time)));
	tmp_pos -= Vector(slide_dist, 0.0f);
	return bool(fabs(tmp_pos.Angle()) <= sensory.MyViewAngle(Self.viewwidth.Data(time)));
}

void MobileObject::forget(){
	forgot = true;
	//pos_conf *= CP_min_valid_conf;
	//speed_conf = 0.0f;
}

void MobileObject::set_speed_param(float max,float decay){
	max_speed = max;
	speed_decay = decay;
}

bool MobileObject::speed_valid(){
	return bool(speed_conf > min_valid_conf);
}

Vector MobileObject::PredictPos(int cycles){
	Vector predictpos = pos;
	Vector vel = global_vel;
	for(int i = 0; i < cycles;i++){
		predictpos += vel;
		vel *= speed_decay;
	}
	return predictpos;
}

void MobileObject::PredictPos(Vector p[], int cycles){
	if(cycles <= 0) return;
	Vector vel = global_vel;
	p[0] = pos + vel;
	for(int i = 1; i < cycles;i++){
		vel *= speed_decay;
		p[i] = p[i-1] + vel;
	}
}

float MobileObject::GetOffsensitivity(){
	return GetOffsensitivity(situation.CurrentTime);
}

float MobileObject::GetOffsensitivity(Time t){
	if(!offsensitivity.IsDataKnown(t)){
		offsensitivity.Setdata(fieldinfo.GetOffensiveSensitivity(Pos(t)), t);
	}
	return offsensitivity.Data(t);
}

float MobileObject::GetDefsensitivity(){
	return GetDefsensitivity(situation.CurrentTime);
}

float MobileObject::GetDefsensitivity(Time t){
	if(!defsensitivity.IsDataKnown(t)){
		defsensitivity.Setdata(fieldinfo.GetDefensiveSensitivity(Pos(t)), t);
	}
	return defsensitivity.Data(t);
}

bool MobileObject::IsLastSeen(){
	return bool(Update_seen_time() == Self.Update_seen_time()) ;
}
/**************************   Player    ******************************/
Player::Player(){
	MobileObject::MobileObject();
	InsideNO = -1;
	headfacing = 0;
	bodyfacing = 0;
	IT_inf.IT_cycles = InfCycles;
	Is_goalie = false;
	No = -1;
	side = Side_Unknown;
	Iscontrolball = false;
	original_postime = 0;
	original_veltime = 0;
}

void Player::Reset(Time time){
	vel_time = time - 1;
	gvel.Setdata(0, time -1);
	speed_conf = 0;
	update_seen_time = Max(time - 1, 0);

	pos_time = time -1;
	gpos.Setdata(0, pos_time);
	
	coordinate_time = -1;
}

bool Player::IsSamePlayer(Player& newp){
	float dist;
	int timegap;
	if(gpos.IsDataKnown(original_postime)){ 
		dist = Pos(original_postime).dist(newp.Pos(newp.original_postime));
		timegap = abs(newp.original_postime - original_postime);
	}else{
		dist = pos.dist(newp.pos);
		timegap = 8;
	}
	if(dist > CP_too_different_distance) return false;
	if(dist < ((newp.distance + distance) * SP_dist_qstep + timegap * SP_player_speed_max + 1.0f))
		return true;
	else
		return false;
}

void Player::estimaters(Time time){
	if(!rs_valid) return;

	while(rspos_time < time){
		rspos.Setdata(rspos.Data(rspos_time), rspos_time+1);
		rsradius.Setdata(Min(rsradius.Data(rspos_time) + max_speed, CP_latent_max_area), rspos_time+1);
		rspos_time ++;
		//DoLog(LOG_VDEC, "estimate %d (%.1f, %.1f) r%.1f at %d", InsideNO,rspos.Data(rspos_time).x, rspos.Data(rspos_time).y, rsradius.Data(rspos_time), rspos_time);
	}
}

void Player::update(Time time){
	if(InsideNO == MyNumber) return;

	MobileObject::update(time);
	//check if it control the ball
	if (ball.IsSeen()){
	//see ball
		Iscontrolball = seen && (Pos(ball.seen_time()).dist2(ball.Pos(ball.seen_time())) < Sqr(SP_kickable_area));
	}
}

void Player::update_end(){
	if(sensory.NewSight){// && IsUNumKnown()){//don't update unknown player
		//update active area
		if(seen){			
			DoLog(LOG_VISUAL, "%d is seen", InsideNO);
		}else if(rs_valid){
			estimaters(sensory.LastSightTime);
			//split circle(rspos, rsradius) with my view angle(headfcing, viewangle)
			Vector relpos = rspos.Data(sensory.LastSightTime) - Self.Pos(sensory.LastSightTime);
			if(relpos.mod() > SP_team_far_length) return;
			
			relpos = relpos.Rotate(-Self.Headfacing(sensory.LastSightTime));
			float gapangle = relpos.Angle();
			float rayangle = gapangle > 0 ? sensory.MyViewAngle() : -sensory.MyViewAngle();
			Ray viewray(Vector(0,0), rayangle);
			Vector pos1,pos2; 
			if(fabs(gapangle) < sensory.MyViewAngle()+1.0f){
				DoLog(LOG_VISUAL,"Player%d (%.1f, %.1f) radius %.1f is out of range ", InsideNO, rspos.Data(sensory.LastSightTime).x, rspos.Data(sensory.LastSightTime).y, rsradius.Data(sensory.LastSightTime));
			}
			
			if(!viewray.CircleIntersect(rsradius.Data(sensory.LastSightTime), relpos,pos1, pos2)){//no split
				if(fabs(gapangle) < sensory.MyViewAngle()+1.0f){
					rs_valid = false;
					DoLog(LOG_VISUAL,"Player%d rs fail agl%.0f, dist%.1f conf %.1f", InsideNO, gapangle, relpos.mod(), pos_conf);
				}
			}else{
			}
		}
	}
	estimaters(situation.CurrentTime);