objects.cpp

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

			float verroridx;
			bool PrevelError = IsposError(allowvelgap, predictvel_gap, verroridx);
			if(PredictError || PrevelError){
				erroridx += verroridx;
				DoLog(LOG_UPDATE, "Error Index %.3f",erroridx);
			}
			error_idx.Setdata(erroridx, t);
			continue;
		}
		if(predictpos.mod() < SP_ball_size + SP_player_size){
			DoLog(LOG_UPDATE, "predict coll, unhandled yet");
		}
		error_idx.Setdata(erroridx, t);
		float sqrconf = max_conf * Sqr(conf_decay);
		float CP_collision_maxdis = 0.75f;
		
		if(!PredictError ){
			if(postime_gap ==1 && !OtherKick(last_seen_time,t) && velgap.IsDataKnown(t) && (posgap.Data(t).mod() + posgap.Data(t-1).mod()) > velgap.Data(t).mod()){
				DoLog(LOG_UPDATE, "estimate maybe more accurate, reset velconf");
				speed_conf = max_conf;
				//wait for estimate
			}else if(selfcoll && postime_gap <= 2){
				DoLog(LOG_UPDATE, "self coll, pos still correct fortunately");
				//wait for estimate
			}else if(!gpos.IsDataKnown(last_seen_time)){
				DoLog(LOG_UPDATE, "last pos is lost, have to estimate");
				//wait for estimate
			}else if(postime_gap<4 && speed_valid()){
				ApplyPosbased_vel(last_seen_time,t,max_conf);
			}else if(postime_gap<6){
				ApplyPosbased_vel(last_seen_time,t,sqrconf);
			}else
				ApplyPosbased_vel(last_seen_time,t,sqrconf * sqrconf);
			
			if(!selfcoll && postime_gap == 1 && seen_distance(t) < 2.0f){
				original_veltime = t - 1;
				DoLog(LOG_UPDATE,"Reliable predict vel at %d", t);
			}
			continue;
		}else{
			DoLog(LOG_UPDATE,"why ball is here, Error Index %.3f at %d", erroridx, t);
		}

		//unmoved posgap limit;
		float dist_moved;
		if(gpos.IsDataKnown(last_seen_time)){
			dist_moved = (gpos.Data(last_seen_time) + Self.SummarizePosGap(last_seen_time, t)).dist(gpos.Data(t)) - allowposgap.mod();
		}else{
			dist_moved = predictpos.dist(gpos.Data(t)) - Vel(t).mod() * postime_gap  - allowposgap.mod();
		}
		if(dist_moved > max_speed * postime_gap){
			DoLog(LOG_UPDATE,"ball is moved");
			gvel.Setdata(0, t);
			vel_time = t;
			if(postime_gap ==1){
				speed_conf = max_conf;
			}else if(postime_gap == 2){
				speed_conf = sqrconf;
			}
		}else if(postime_gap ==1){
			if(OtherKick(last_seen_time)){
				ApplyPosbased_vel(last_seen_time,t,sqrconf);
				DoLog(LOG_UPDATE,"Predict other kick at %.0f",(float)last_seen_time);
			}else if(selfcoll){
				//wait for estimate
				DoLog(LOG_UPDATE,"Error because Self coll");
			}else if(last_seench_time < last_seen_time-1 ){
				ApplyPosbased_vel(last_seen_time, t, sqrconf );
				DoLog(LOG_UPDATE,"Error maybe too old vel");
			}else if(seen_distance(t) < CP_collision_maxdis){
				Collisionvel(t,sqrconf);
				DoLog(LOG_UPDATE,"It must be coll");
			}else{
				ApplyPosbased_vel(last_seen_time, t, sqrconf);
				DoLog(LOG_UPDATE,"Unknown reason");
			}
		}else if(postime_gap == 2){
			if(OtherKick(last_seen_time)){
				ApplyPosbased_vel(last_seen_time, t, sqrconf * conf_decay);
				DoLog(LOG_UPDATE,"Predict other kick2 at%d", last_seen_time);
			}else if(OtherKick(last_seen_time + 1)){
				ApplyPosbased_vel(last_seen_time + 1, t, sqrconf * conf_decay);
				DoLog(LOG_UPDATE,"Predict other kick3 at%d", last_seen_time+1);
			}else if(selfcoll){
				//wait for estimate
				DoLog(LOG_UPDATE,"Error because Self coll");
			}else if(last_seench_time<last_seen_time-1){
				ApplyPosbased_vel(last_seen_time, t, sqrconf * conf_decay );
				DoLog(LOG_UPDATE,"Error maybe too old vel");
			}else{
				if(seen_distance(t) > SP_feel_distance){
					DoLog(LOG_UPDATE,"too far ,see wrong");
				}else{
					float dis1 = (Pos(t) - Self.Pos(t)).mod() ;
					float dis2 = (Pos(t-1) - Self.Pos(t-1)).mod();
					DoLog(LOG_UPDATE,"t%d dis%.2f, t%d dis%.2f", t-1, dis2, t,dis1);
					if(dis1 < dis2 && dis1 < CP_collision_maxdis){
						Collisionvel(t, sqrconf * conf_decay);
						DoLog(LOG_UPDATE,"find coll at t%d", t);
					}else if(dis2 < dis1 && dis2 < CP_collision_maxdis){
						Collisionvel(t - 1, sqrconf * conf_decay);
						DoLog(LOG_UPDATE,"find coll at t%d", t-1);
					}else{
						if(action.KickEffect(last_seen_time).mod() > 0 && ball.Pos(last_seen_time).dist2(Self.Pos(last_seen_time)) > 1.0f){
							DoLog(LOG_UPDATE,"maybe kick%d invalid", last_seen_time+1);
							action.ResetKickEffect(last_seen_time+1);
						}else{
							DoLog(LOG_UPDATE,"maybe unfound kick %d", t);
						}

						ApplyPosbased_vel(last_seen_time, t, sqrconf * conf_decay );
						//invalid kick not be clear out. or other kick haven't been found
					}
				}
			}
		}else if(!gpos.IsDataKnown(last_seen_time)){
			//wait for estimate
		}else if(postime_gap<6){
			ApplyPosbased_vel(last_seen_time, t, sqrconf * conf_decay);
		}else{
			ApplyPosbased_vel(last_seen_time, t, CP_min_valid_conf * conf_decay);
		}
	}
}

void Ball::ApplyPosbased_vel(Time prevtime,Time latestime,float conf){
	bool valid;
	Vector evel_posbased;
	evel_posbased = posbased_vel_estimate(prevtime, latestime, valid);
	if(valid){
		gvel.Setdata(evel_posbased, latestime);
		vel_time = latestime;
		speed_conf = conf;
		DoLog(LOG_UPDATE,"t%d, Posbased vel applied", latestime);
	}else{
		DoLog(LOG_UPDATE,"Why posbased_vel Invalid (%.2f %.2f)", evel_posbased.x, evel_posbased.y);
	}
	velgap.Setdata(posgap.ChkData(prevtime) + posgap.ChkData(latestime), latestime);
	velgap_time = latestime;
	DoLog(LOG_UPDATE, "reset velgap (%.2f, %.2f)",velgap.Data(latestime).x, velgap.Data(latestime).y);
}

bool Ball::Heardupdate(){
	if(!heard) return false;	

	if (pos_conf > 0.8f || distance < 2.0f){
		//it is critical for me, if the confidence of ball is greater than 0.8f and it ball is within 2 meter of my reach
		if (heardvelinfo.time != heardposinfo.time)	return false;

		if (ball.global_vel.dist2(heardvelinfo.data) < Sqr(0.6f)) return false;
	}

	if(ball.heardpass.time == heardposinfo.time	&& heard_org_postime >= original_postime)
		return true;

	if(heard_org_postime <= original_postime)
		return false;
		
	if(IsLastSeen() && !OtherKick(update_seen_time)) return false;

	return true;
}

void Ball::update_end(){	
	MobileObject::update_end(); 
	if(distance > CP_kickable_area || !pos_valid() || !Rs_valid()){
		kick_able = false;
	}else{
		kick_able = (Update_seen_time() >= Self.Update_seen_time() || 
			ball.Pos(Self.Update_seen_time()).dist(Self.Pos(Self.Update_seen_time())) + ball.posgap.ChkData(Self.Update_seen_time()).mod() > SP_feel_distance);
	}
	if(distance < CP_kickable_area && !kick_able){
		if(!pos_valid()) DoLog(LOG_UPDATE, "kick disable for pos invalid");
		else if(!Rs_valid()) DoLog(LOG_UPDATE, "kick disable for Rs invalid");
		else{
			DoLog(LOG_UPDATE, "kick disable for ball( %d) is unseen at %d", Update_seen_time(), Self.Update_seen_time());
		}
	}
	theirgoalangle = (fieldinfo.theirgoal - pos).Angle();
	mygoalangle = (fieldinfo.mygoal - pos).Angle();
}
 
void Ball::Collisionvel(Time time,float conf){
	Vector vel = action.SummarizeKickeffectVel(Vel(vel_time),vel_time,time,speed_decay,max_speed);
	gvel.Setdata(vel * -0.1f, time);
	vel_time = time;
	speed_conf = conf;
	DoLog(LOG_UPDATE,"coll handle t%.0f,v(%.2f,%.2f)",(float)time,Vel(time).x,Vel(time).y);
}

bool Ball::OtherKick(Time time){
	if(IscrposKnown(time)){
		if(closestrpos(time).mod() < SP_kickable_area+0.05f )
			return true;
	}
	return false;
}

 bool Ball::OtherKick(Time prevtime,Time lastime){
	for(Time t=prevtime;t<=lastime;t++){
		if(OtherKick(t)) return true;
	}
	return false;
}

bool Ball::IsposError(Vector allowposgap, Vector predictgap,float& gapmod){
	if(fabs(predictgap.x) > allowposgap.x || fabs(predictgap.y) > allowposgap.y){
		gapmod = fabs(predictgap.x) > allowposgap.x ? Sqr((float)fabs(predictgap.x)-allowposgap.x) : 0;
		gapmod += fabs(predictgap.y) > allowposgap.y ? Sqr((float)fabs(predictgap.y)-allowposgap.y) : 0;
		gapmod = (float)sqrt(gapmod / allowposgap.mod());
		return true;
	}else{
		gapmod = 0;
		return false;
	}
}

Vector Ball::posbased_vel_estimate(Time prev_time, Time last_time, bool& valid){
	/* estimate by postion difference */
	valid = false;
	Vector gap = Pos(last_time) - Pos(prev_time);
	Vector evel;
	gap -= Self.SummarizePosGap(prev_time, last_time);

	DoLog(LOG_UPDATE,"prev pos (%.2f %.2f) last pos (%.2f %.2f) gap (%.2f %.2f)", Pos(prev_time).x, Pos(prev_time).y,
		Pos(last_time).x, Pos(last_time).y, gap.x, gap.y);
	//if (gap.mod() < (float)fabs(last_time - prev_time) * max_speed){
	Vector kickeffectpos = action.SummarizeKickeffectPos(prev_time, last_time, speed_decay);
	evel = (gap - kickeffectpos) * (1 - speed_decay) / (1 - Exp(speed_decay, (float)(last_time - prev_time)));
	//evel is now that at time prev_time
	if(evel.mod() > max_speed){
		evel = evel / evel.mod() * max_speed;
	}
	evel = action.SummarizeKickeffectVel(evel, prev_time, last_time, speed_decay, max_speed);
	valid = true;
	return evel;
}

void Ball::estimate(Time time){
	MobileObject::estimate(time);
	estimategap(time);
}

void Ball::estimate_pos(Time time){
	MobileObject::estimate_pos(time);
}

void Ball::CollisionHandle(Time time){
	DoLog(LOG_UPDATE,"Coolisionhandle");
	estimate_pos(time);
	if (speed_valid()){
		Vector rpos = Pos(time) - Self.Pos(time);
		float r = SP_ball_size + SP_player_size;
		float d = rpos.mod();
		if (d>=r) return; 
		AngleDeg th = float(fabs(NormalizeAngle(rpos.Angle() - Self.Bodyfacing(time -1) - 180.0f)));
		float l1 = d * Cos(th);
		float h = d * Sin(th);
		float cosp = h / r;
		float sinp = (float)sqrt(1.0 - Sqr(cosp));
		float l2 = r * sinp;
		gpos.Data(time) += Vel(time) *float( speed_decay * (-(l1+l2) / Max(Vel(time).mod() * speed_decay, 0.000001f)));
		gvel.Data(time) *= -0.1f;
		vel_time = time;
		speed_conf = min_valid_conf / Sqr(conf_decay);
	}
}

bool Ball::catchable(){
	if(Self.Is_goalie)
		return bool(pos_conf > 0.8f && distance < SP_catch_area_l && fieldinfo.my_penaltyarea.IsWithin(Self.pos) && action.cancatch(situation.CurrentTime));
	else
		return bool(pos_conf > 0.8f && distance < SP_catch_area_l && fieldinfo.my_penaltyarea.IsWithin(pos) && action.cancatch(situation.CurrentTime));
}

bool Ball::shootable(){
	return bool(fieldinfo.shootablearea.IsWithin(pos) && kickable());
}

bool Ball::kickable(const Vector& selfpos){
	return bool(pos_valid() && ball.pos.dist2(selfpos) < Sqr(SP_kickable_area));
}

bool Ball::catchable(const Vector& selfpos){
	return bool(pos_valid() && pos.dist2(selfpos) < Sqr(SP_catch_area_l) && fieldinfo.my_penaltyarea.IsWithin(selfpos) && action.cancatch(situation.CurrentTime));
}

bool Ball::shootable(const Vector& ballpos){
	return fieldinfo.shootablearea.IsWithin(ballpos);
}

void Ball::Setball_CPlayer(UNum idx,Vector pos,Time time){
	if(idx > 0 && idx <= 2*SP_team_size)
		ClosestPlayer.Setdata(idx, time);
	else
		ClosestPlayer.Setdata(idx, time-1);
	ClosestRPos.Setdata(pos-Pos(time),time);
}

bool Ball::IsOtherKick(){
	if(IsOtherKick(situation.CurrentTime) || IsOtherKick(situation.CurrentTime - 1)){
		return true;
	}else
		return false;
}

bool Ball::IsOtherKick(Time t){
	if(error_idx.IsDataKnown(t) && error_idx.Data(t) > 0.01f )
		return true;
	else
		return false;
}
/*	UnknownPlayer	*/
UnknownPlayerNode::UnknownPlayerNode(){
	next = NULL;
}

/*	UnknownPlayerList	*/
UnknownPlayerList::UnknownPlayerList(){
	Head.next = NULL;
	Tail = &Head;
	numplayers = 0;
}

UnknownPlayerList::~UnknownPlayerList(){
	UnknownPlayerNode* tmp;
	while(Head.next != NULL){
		tmp = Head.next->next;
		delete Head.next;
		Head.next = tmp;
	}
}

void UnknownPlayerList::EnList(Player& player){
	if(sensory.TwiceSight){//check if repeat player
		for(int i=oldnumplayers; i<numplayers; i++){
			if(player.seen_time() == List[i]->seen_time()) break;
			if(List[i]->IsSamePlayer(player)){
				DoLog(LOG_UNKPLAYER,"find two same unknown opp");
				return;
			}
		}
	}
	if(numplayers >= SP_team_size){
		DoLog(LOG_UNKPLAYER,"num_unk_players reach limts");
		return;
	}

	Tail->next = new UnknownPlayerNode;
	Tail->next->unknownplayer = player;
	if(!IsUnknownside()){
		Tail->next->unknownplayer.set_side(IsMyside());
	}
	Tail = Tail->next;
	Tail->next = NULL;
	ArraySet(&(Tail->unknownplayer));
}

bool UnknownPlayerList::Clearable(Player& player){
	if(situation.ClockStopped) return true;
	if(player.pos_conf * Exp(CP_conf_decay, (float)(situation.CurrentTime - player.pos_time)) < 0.8f) return true;

	if(!sensory.NewSight) return false;
	if (player.inview(sensory.LastSightTime) || (sensory.TwiceSight && player.inview(sensory.PrevSightTime))){
		if(IsUnknownside()) return true;
		if(player.distance < SP_team_far_length) return true;
	}
	int i;
	float max_deviation = CP_different_distance;
	if(IsMyside() || IsUnknownside()){
		for(i=0; i<SP_team_size; i++){
			if(!MyTeam[i].IsSeen()) continue;
			if(player.pos.disaeolus(MyTeam[i].pos) < max_deviation) return true;
		}
		for(i=0; i<num_unknown_myplayers; i++){
			if (!UnknownMyPlayers[i].IsDistanceSeen()) continue;
			if(player.pos.disaeolus(UnknownMyPlayers[i].pos) < max_deviation) return true;
		}
	}
	if(IsOppside() || IsUnknownside()){
		for(i=0; i<SP_team_size; i++){
			if(!TheirTeam[i].IsSeen()) continue;
			if(player.pos.disaeolus(TheirTeam[i].pos) < max_deviation) return true;
		}
		for(i=0; i<num_unknown_theirplayers; i++){
			if (!UnknownTheirPlayers[i].IsDistanceSeen()) continue;
			if(player.pos.disaeolus(UnknownTheirPlayers[i].pos) < max_deviation) return true;
		}
	}
	if(IsUnknownside()){
		for(i=0; i<num_teamlessplayers; i++){
			if (!TeamlessPlayers[i].IsDistanceSeen()) continue;
			if(player.pos.disaeolus(TeamlessPlayers[i].pos) < max_deviation) return true;
		}
	}
	return false;
}

void UnknownPlayerList::ArraySet(Player* player){
	List[numplayers ++] = player;
}

void UnknownPlayerList::update_end(){
	for(int i=0; i< numplayers; i++){
		List[i]->update_end(); 
	}
} 

void UnknownPlayerList::update(Time time){
	UnknownPlayerNode* p = &Head, *tmp;
	numplayers = 0;	
	while(p->next != NULL){		
		if (Clearable(p->next->unknownplayer)){
			tmp = p->next->next;
			delete p->next;
			p->next = tmp;
		}else{
			p = p->next;
			p->unknownplayer.update(time);
			ArraySet(&(p->unknownplayer));
		}
	}
	Tail = p;
	oldnumplayers = numplayers;
}

void UnknownPlayerList::LogList(char* msg){
	UnknownPlayerNode* p = &Head;	
	int logcount=0;
	if(msg != NULL)
		logcount += sprintf(logbuf,msg);
	else{
		if(IsMyside()){
			logcount += sprintf(logbuf,"Teammate %d", numplayers);
		}else if(IsOppside()){
			logcount += sprintf(logbuf,"Opp %d", numplayers);
		}else{
			logcount += sprintf(logbuf,"Unknown %d", numplayers);
		}
	}
	while(p->next != NULL){
		p = p->next;
		logcount += sprintf(&logbuf[logcount]," pos(%.2f,%.2f)",p->unknownplayer.pos.x, p->unknownplayer.pos.y);
	}
	DoLog(LOG_UNKPLAYER,logbuf);
}

int UnknownPlayerList::NumUnknownPlayers(){
	return numplayers;
}

void UnknownPlayerList::Set_IsControlball_FromHeard(Time time){
	UnknownPlayerNode* p = &Head;
	while(p->next != NULL){
		p->next->unknownplayer.Set_Iscontrolball_Fromheard(time);
		p = p->next;
	}
}