objects.cpp

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

	MobileObject::update_end();

	rel_pos_2_ball = pos - ball.pos;
	balldist = rel_pos_2_ball.mod();
	ballangle = (ball.pos - pos).Angle();
	if(rs_valid){
		DoLog(LOG_UPDATE, "estimate %d (%.1f, %.1f) r%.1f", // sensitivity(%.2f, %.2f)",
			InsideNO, rspos.Data(rspos_time).x, rspos.Data(rspos_time).y, rsradius.Data(rspos_time));//, GetOffsensitivity(), GetDefsensitivity());
		//DoLog("rel_pos_2_ball %d (%.2f %.2f) r %.1f", InsideNO, rel_pos_2_ball.mod(), rel_pos_2_ball.Angle(), rsradius.Data(rspos_time));
	}
	//if(Is_goalie) pos_conf = CP_min_valid_conf;
}

bool Player::Heardupdate(){
	if(!heard || seen) return false;

	if (heard_org_postime <= original_postime) return false;
	if(pos_valid() && rs_valid && rspos.IsDataKnown(situation.CurrentTime -1)){
		if(rspos.Data(situation.CurrentTime-1).dist(heardposinfo.data) > (ActiveRadius(situation.CurrentTime -1) + SP_player_speed_max) + SP_dist_qstep * distance * 1.5f){
			DoLog(LOG_HEAR, "Wronginfo %d(%.2f, %.2f) at %d to (%.2f %.2f) at %d, dist%.2f, radius %.2f", InsideNO, heardposinfo.data.x, heardposinfo.data.y,heard_org_postime,
			rspos.Data(situation.CurrentTime-1).x, rspos.Data(situation.CurrentTime-1).y, original_postime,rspos.Data(situation.CurrentTime-1).dist(heardposinfo.data), (ActiveRadius(situation.CurrentTime -1) + SP_player_speed_max) * 1.2f);
			return false;
		}
	}
	return true;
}

AngleDeg Player::Bodyfacing(Time time){
	return bodyfc.IsDataKnown(time) ? bodyfc.Data(time) : bodyfacing;
}

AngleDeg Player::Headfacing(Time time){
	return headfc.IsDataKnown(time) ? headfc.Data(time) : headfacing;
}

AngleDeg Player::Headangle(Time time){
	return headang.IsDataKnown(time) ? headang.Data(time) : head_angle;
}

bool Player::IsBodyfacingKnown(Time time){
	return bodyfc.IsDataKnown(time);
}

bool Player::IsHeadfacingKnown(Time time){
	return headfc.IsDataKnown(time);
}

bool Player::IsHeadAngleKnown(Time time){
	return headang.IsDataKnown(time);
}

void Player::set_fcinfo(float facedir, float neckdir, Time time){
	fcinfo = TimedData<Vector>(Vector(facedir, neckdir), time);
}

void Player::set_inside_no(UNum No){
	this->InsideNO = No;
}

void Player::set_side(bool Is_myside){
	if (Is_myside){
		side = Side_My;
	}
	else{
		side = Side_Opp;
	}
}

void Player::set_side(PlayerSide side){
	this->side = side;
}

void Player::Set_Iscontrolball_Fromheard(Time time){
	if (heardposinfo.time == time && heard_iscontrolball){
		Iscontrolball = true;
	}
	else{
		Iscontrolball = false;
	}
}

bool Player::IsStrained(){
	return bool(stamina < 0.5f * SP_stamina_max);
}

bool Player::IsExtremStrained(){
	return bool(stamina < SP_stamina_max * SP_recover_dec_thr);
}

/**************************   Self   *********************************/
MySelf::MySelf(){
}

void MySelf::update(Time time){
/*********	time is assumed to be CurrentTime here	********/
	if (sensory.SensedInfKnown(time)){
		update_from_senseinfo(sensory.GetSenseBodyInfo(time));
	}else{
		estimate_headangle(time);
		estimate_stamina(time);
		estimate_effort(time);
	}

	update_seen_facing(time);
	estimate_facing(time);

	update_vel(time);// vel should be updated before pos
	update_seen_pos(time);
	estimate_pos(time);

	pos = Pos(time);
	global_vel = Vel(time);
	speed = global_vel.mod();
	vel_angle = global_vel.Angle();
	bodyfacing = Bodyfacing(time);
	headfacing = Headfacing(time);
	head_angle = Headangle(time);
	stamina = Stamina(time);
	effort = Effort(time);
	pos_time = time;
	vel_time = time;
}

void MySelf::update_from_senseinfo(SenseBodyInfo & sb){
	viewwidth.Setdata(sb.view_width, sb.time);
	viewqual.Setdata(sb.view_qual, sb.time);
	stmn.Setdata(sb.stamina, sb.time);
	spd.Setdata(sb.speed < max_speed * speed_decay ? sb.speed : max_speed * speed_decay, sb.time);
	spdang.Setdata(sb.rel_spd_angle, sb.time);
	headang.Setdata(sb.head_angle, sb.time);
	efft.Setdata(sb.effort, sb.time);
}

void MySelf::estimate_headangle(Time time){
	if (action.Valid(CMD_turn_neck)){
		headang.Setdata(Headangle(situation.LastCycle()) + action.TurnNeckEffect(situation.LastCycle()), time);
	}
}

void MySelf::estimate_stamina(Time time){
	stmn.Setdata(Stamina(situation.LastCycle()) - action.UsedPower(situation.LastCycle()), time);
}

void MySelf::estimate_effort(Time time){
	efft.Setdata(Effort(situation.LastCycle()), time);
}

void MySelf::update_seen_facing(Time time){
	if (!sensory.NewSight) return;

/******	update facings from seen	************/
	int last_update_seen_time = Max(update_seen_time, time -2);
	for(int t = last_update_seen_time + 1; t <= sensory.LastSightTime; t ++){
		if (sensory.num_seenlines.ChkData(t) > 0){
			int line = sensory.SeenLines.Data(0, t);
			float rel_angle_2_line = Lines[line].seen_rel_angle(t);
			if (rel_angle_2_line < 0){
				rel_angle_2_line += 180.0f;
			}

			headfc.Setdata(NormalizeAngle(Lines[line].global_angle - rel_angle_2_line), t);
			bodyfc.Setdata(NormalizeAngle(Headfacing(t) - Headangle(t)), t);
		}
		else if (sensory.num_seenmarkers.ChkData(t) >= 2 ){
			//find two markers with known distance
			MarkerType marker1, marker2;
			marker1 = sensory.SeenMarkers.Data(0, t);
			marker2 = sensory.SeenMarkers.Data(1, t);

			Markers[marker1].rel_pos = Polar2Vector(Markers[marker1].seen_distance(t), Markers[marker1].seen_rel_angle(t));
			Markers[marker2].rel_pos = Polar2Vector(Markers[marker2].seen_distance(t), Markers[marker2].seen_rel_angle(t));
			Vector gap1 = Markers[marker1].rel_pos - Markers[marker2].rel_pos;
			Vector gap2 = Markers[marker1].pos - Markers[marker2].pos;
			headfc.Setdata(NormalizeAngle(gap2.Angle() - gap1.Angle()),	t);
			bodyfc.Setdata(NormalizeAngle(Headfacing(t) - Headangle(t)), t);
		}
	}
}

void MySelf::estimate_facing(Time time){
	if(!IsBodyfacingKnown(time)){
		if (!IsBodyfacingKnown(situation.LastCycle())){
			/********	must miss some cycles	************/
			bodyfc.Setdata(NormalizeAngle(Bodyfacing(pos_time) + action.SummarizeTurneffect(pos_time, situation.LastCycle()))
				, situation.LastCycle());
			if(IsHeadAngleKnown(situation.LastCycle())){
				headang.Setdata(NormalizeAngle(Headangle(pos_time) + action.SummarizeTurnneckeffect(pos_time, situation.LastCycle()))
					, situation.LastCycle());
			}
			headfc.Setdata(NormalizeAngle(Bodyfacing(situation.LastCycle()) + Headangle(situation.LastCycle())), 
					situation.LastCycle());
		}

		bodyfc.Setdata(NormalizeAngle(Bodyfacing(situation.LastCycle()) + action.TurnEffect(situation.LastCycle())), time);
		headfc.Setdata(NormalizeAngle(Bodyfacing(time) + Headangle(time)), time); 
	}
}

void MySelf::update_vel(Time time){
	Vector evel = action.SummarizeDasheffectVel(Vel(vel_time), vel_time, time, speed_decay, max_speed);

	if (sensory.SensedInfKnown(time)){
		gvel.Setdata(Polar2Vector(Speed(time), SpeedAngle(time) + Headfacing(time)), time);

		float Velgap =(float)fabs(gvel.Data(time).mod() - evel.mod());

		Vector svgap = gvel.Data(time) - evel;

		if(Velgap >CP_selfvel_mingap ){
			DoLog(LOG_COLLIDE,"self coll Velgap%.2f,Vector Gap(%.2f,%.2f)",Velgap,svgap.x,svgap.y);
			selfcoll.Setdata(true,time);
		}else
			selfcoll.Setdata(false,time);
	}else{
		/*estimate vel*/
		gvel.Setdata(evel, time);
	}
	vel_time = time;
}

void MySelf::update_seen_pos(Time time){
/********	Update Pos from seen	**************/
	if(!sensory.NewSight) return;

	int last_update_seen_time = Max(update_seen_time, time -2);
	MarkerType closestmarker;
	for(int t = last_update_seen_time + 1; t <= sensory.LastSightTime; t ++){
		if (sensory.num_seenmarkers.ChkData(t) > 0){
			/***********	calculate pos gap	***************/
 			closestmarker = sensory.GetClosestMarker(t);
			if (closestmarker != No_Marker){
				Markers[closestmarker].rel_pos = Polar2Vector(Markers[closestmarker].seen_distance(t),
					Headfacing(t) + Markers[closestmarker].seen_rel_angle(t));
				gpos.Setdata(Markers[closestmarker].pos - Markers[closestmarker].rel_pos, t);
				DoLog(LOG_UPDATE, "Player Pos Updated (%.2f %.2f) at %d", Pos(t).x, Pos(t).y, t);

				if(pos_time < t -1){
					estimate_pos(t-1);
				}
				Vector posbk = Pos(t-1) + Vel(t) / speed_decay;
				pos_gap.Setdata(Pos(t) - posbk, t);
				if(pos_gap.Data(t).mod() > 4.0f){
					DoLog("I'm moved");
					pos_gap.Setdata(0, t);
				} 
				DoLog(LOG_UPDATE, "PrevCoord (%.2f %.2f)", posbk.x, posbk.y);				
				pos_time = t;
				update_seen_time = t;
			}
		}
	}
}


void MySelf::estimate_pos(Time time){
	if (pos_time < time){
		gpos.Setdata(Pos(pos_time) + SummarizeVelPos(pos_time, time), time);
		pos_time = time;
	}
}

Vector MySelf::SummarizeVelPos(Time prev, Time last){
	Vector moved_pos = 0;
	for(Time time = prev; time < last; time ++){
		moved_pos += Vel(time + 1) / speed_decay;
	}
	return moved_pos;
}

Vector MySelf::SummarizePosGap(Time prev, Time last){
	Vector moved_pos = 0;
	for(Time time = prev + 1; time <= last; time ++){
		moved_pos += pos_gap.ChkData(time);
	}
	return moved_pos;
}

float MySelf::Speed(Time time){
	return spd.IsDataKnown(time) ? spd.Data(time) : speed;
}

AngleDeg MySelf::SpeedAngle(Time time){
	return spdang.IsDataKnown(time) ? spdang.Data(time) : NormalizeAngle(vel_angle - headfacing);
}

float MySelf::Stamina(Time time){
	return stmn.IsDataKnown(time) ? stmn.Data(time) : stamina;
}

float MySelf::Effort(Time time){
	return efft.IsDataKnown(time) ? efft.Data(time) : effort;
}

bool MySelf::SelfColl(Time prevtime,Time lastime ){
	for(Time t=prevtime; t<=lastime;t++){
		if(selfcoll.ChkData(t)) return true;
	}
	return false;
}
bool MySelf::IsSpeedKnown(Time time){
	return spd.IsDataKnown(time);
}
bool MySelf::IsSpeedAngleKnown(Time time){
	return spdang.IsDataKnown(time);
}

bool MySelf::IsStaminaKnown(Time time){
	return stmn.IsDataKnown(time);
}

bool MySelf::IsEffortKnown(Time time){
	return efft.IsDataKnown(time);
}

Vector MySelf::PredictPos(float dash_pow, int cycles){
	Vector predictpos = pos;
	Vector vel = global_vel;
	for(int i = 0; i < cycles; i ++){
		vel += Polar2Vector(dash_pow, Self.bodyfacing) * action.Dashrate();
		if (vel.mod() > SP_player_speed_max){
			vel = vel / vel.mod() * SP_player_speed_max;
		}
		predictpos += vel;
		vel *= speed_decay;
	}
	return predictpos;
}

Vector MySelf::PredictPos(int cycles){
	return MobileObject::PredictPos(cycles);
}

Vector MySelf::PredictPosWithTurnDash(AngleDeg angle, float dash_pow){
	Vector predictpos = Self.PredictPos(2);
	predictpos += Polar2Vector(dash_pow * action.Dashrate(), angle);
	return predictpos;
}

/*********************     Ball     **************************/
Ball::Ball(){
	posgap_time = -1;
	velgap_time = -1;
}

void Ball::update_seengap(Time t){
	if(!seenposinfo.IsDataKnown(t)) return;

	Vector t_posgap;
	float headfacingap = 1.5f;
	float dist = seen_distance(t);
	float rposagl = NormalizeAngle(Self.Headfacing(t) +  seen_rel_angle(t));
	float fcosagl = (float)fabs(Cos(rposagl));
	float fsinagl = (float)fabs(Sin(rposagl));
	//相对距离与角度误差
	float dis_gap = 0.05f + 0.513f * dist * SP_dist_qstep/0.9f;
	float agl_gap = Deg2Rad(0.5f + headfacingap);
	//投影到全局坐标上的误差换算
	t_posgap.x = dis_gap * fcosagl + (dist + dis_gap) * agl_gap * fsinagl;
	t_posgap.y = dis_gap * fsinagl + (dist + dis_gap) * agl_gap * fcosagl;
	posgap.Setdata(t_posgap,t);
	
	Vector rpos = Pos(t) - Self.Pos(t);
	DoLog(LOG_UPDATE,"ball rpos(%.4f,%.4f) gap(%.4f %.4f) hafcing %.1f at %d", rpos.x,rpos.y, t_posgap.x, t_posgap.y, Self.Headfacing(t),t);
	posgap_time = t;
	if(seenchinfo.IsDataKnown(t)){
		Vector t_velgap;
		float SP_disch_qstep = 0.02f;
		float SP_dirch_qstep = 0.1f;//SP_dir_qstep;
	
		//相对速度误差
		t_velgap.x = dis_gap / dist * (float)fabs(seen_distCh(t)) + (dist + dis_gap) * SP_disch_qstep * 0.5f;
		t_velgap.y  = dis_gap * Deg2Rad(seen_dirCh(t)) + (dist + dis_gap) * Deg2Rad(SP_dirch_qstep * 0.5f);
		
		t_velgap = t_velgap.Rotate(rposagl);
		t_velgap.x = (float)fabs(t_velgap.x);
		t_velgap.y = (float)fabs(t_velgap.y);
		//旋转rposagl引入的误差
		float tmp = t_velgap.y;
		t_velgap.y += t_velgap.x * agl_gap;
		t_velgap.x += tmp * agl_gap;
		
		//Self的速度误差
		Vector svelgap;
		float rvelagl = NormalizeAngle(Self.Headfacing(t) + Self.SpeedAngle(t));
		fcosagl = (float)fabs(Cos(rvelagl));
		fsinagl = (float)fabs(Sin(rvelagl));
		svelgap.x = (Self.Speed(t) + 0.005f) * agl_gap * fsinagl;
		svelgap.y = (Self.Speed(t) + 0.005f) * agl_gap * fcosagl;
		t_velgap += svelgap;
		velgap.Setdata(t_velgap,t);
		velgap_time = t;

		DoLog(LOG_UPDATE,"ball vel(%.4f,%.4f) gap(%.4f %.4f) selfvel gap(%.2f,%.2f)",Vel(t).x,Vel(t).y,t_velgap.x,t_velgap.y,svelgap.x,svelgap.y);
	}
}

void Ball::estimategap(Time t){
	Time i;
	Vector gap;
	for(i = velgap_time; i < t; i++){
		gap = (velgap.ChkData(i) + Vector(1,1) * SP_ball_rand * (velgap.ChkData(i).mod() + gvel.ChkData(i+1).mod()/speed_decay)) * speed_decay;
		gap.x = gap.x > max_speed ? max_speed : gap.x;
		gap.y = gap.y > max_speed ? max_speed : gap.y;
		velgap.Setdata(gap, i+1);
		DoLog(LOG_UPDATE,"Estimate velgap (%.3f,%.3f) at %d", velgap.ChkData(i+1).x, velgap.ChkData(i+1).y, i+1);
	}
	velgap_time = t;
	DoLog(LOG_UPDATE, "Estimate posgap (%.3f,%.3f) at %d(%.2f %.2f %.2f)",posgap.ChkData(posgap_time).x, posgap.ChkData(posgap_time).y, posgap_time, speed_decay, SP_player_rand, SP_player_decay);
	for(i = posgap_time; i < t; i++){
		posgap.Setdata(posgap.ChkData(i) + velgap.ChkData(i+1)/speed_decay + Vector(1,1) * SP_player_rand * Self.Vel(i+1).mod() / SP_player_decay, i+1);
		DoLog(LOG_UPDATE, "Estimate posgap (%.3f,%.3f) selfvel (%.2f,%.2f) at %d",posgap.ChkData(i+1).x, posgap.ChkData(i+1).y, Self.Vel(i+1).x, Self.Vel(i+1).y, i+1);
	}
	posgap_time = t;
}

void Ball::update_seen(Time time){
/* What's special about ball's updation?
	Collisions, and a more accurate velocity is needed*/
	if(!seen) return;
	Time last_update_time = Max(update_seen_time, time -2);	
	Vector predictpos, predictvel;
	Vector allowposgap, allowvelgap;
	Vector predictpos_gap, predictvel_gap;
	
	for(int t = last_update_time + 1; t <= time; t++){
		if(!seenposinfo.IsDataKnown(t)) continue;	
		estimate(t);
		predictpos = Pos(t) - Self.Pos(t);
		DoLog(LOG_UPDATE, "estimate relpos %.2f, %.2f", predictpos.x, predictpos.y);
		predictvel = Vel(t);
		allowposgap = posgap.ChkData(t);
		allowvelgap = velgap.ChkData(t);
		Time last_seen_time = original_postime;
		Time last_seench_time = original_veltime;
		MobileObject::update_seen(t);
		update_seengap(t);
		
		if(seen_distance(t) > 30.0f) continue;
		
		if (seen_distance(t) > (SP_kickable_area+0.04f)){
			action.ResetKickEffect(t); //then the kick command sent at seen_time must be a fault
		}
		int postime_gap = t - last_seen_time;
		if(postime_gap <= 0){
			DoLog(LOG_UPDATE,"why postime_gap = 0? %d", t);
			continue;
		}
		
		allowposgap += posgap.ChkData(t);
		predictpos_gap = Pos(t) - Self.Pos(t) - predictpos;// - Self.SummarizePosGap(last_seen_time, t);
		DoLog(LOG_UPDATE,"T gap%d, V gap%d, Predictpos Gap(%.2f,%.2f)", postime_gap, t - last_seench_time, predictpos_gap.x, predictpos_gap.y);
		
		bool selfcoll = Self.SelfColl(last_seen_time+1, t);
		float erroridx;
		bool PredictError = IsposError(allowposgap, predictpos_gap, erroridx);
		
		if(seenchinfo.IsDataKnown(t)){
			allowvelgap += velgap.ChkData(t);
			predictvel_gap = Vel(t) - predictvel;
			DoLog(LOG_UPDATE,"predictvel Gap(%.2f,%.2f)",predictvel_gap.x,predictvel_gap.y);