mediator.cpp

来自「2002年」· C++ 代码 · 共 514 行 · 第 1/2 页

#include "mediator.h"
#include "worldmodel.h"
#include "perceptron.h"
#include "skill.h"
#include "log.h"
#include "agent.h"

/**************   PrioredCommand *************************/
PrioredCommand::PrioredCommand(){
	Command::Command();
	priority = 0.0f;
	actiontype = Action_none;
}

/*************	VisualRequestQueue	*************************/
VisualRequest::VisualRequest(float priority, AngleDeg angle){
	this->priority = priority;
	this->angle = NormalizeAngle(angle);
}

AngleDeg VisualRequest::RequestedAngle_(){
	return MidAngle(angle,angle_inf);
}

float VisualRequest::RequestedPriority(){
	if(view_width == VW_Narrow) return priority * ClientParam::VW_Narrowfactor;
	else return priority;
}

/**********		Mediator	**************************/
Mediator::Mediator(){
}

void Mediator::ResetBuffer(){
//	visualrequestqueue.cleanup();
//	countervisualrequestqueue.cleanup();
	vr_queue.cleanup();
	ac_queue.cleanup();
	totalvisualrequests = 0;
}


bool Mediator::enroll(PrioredCommand& pcmd){
	DoLog("Enroll cmdtype %d actype %d p %.2f", pcmd.type, pcmd.actiontype, pcmd.priority);
	if(pcmd.type == CMD_none) return false;
	pcmd.priority = priority_mapping(pcmd.actiontype, pcmd.priority);
	return ac_queue.enqueue(pcmd);
}

bool Mediator::enroll(Command& command, ActionType actiontype, float priority){
	PrioredCommand pricmd = PrioredCommand(command, actiontype, priority);
	return enroll(pricmd);
}

void Mediator::Getbestaction(PrioredCommand& bestaction){
	bestaction.Reset();
	if(!ac_queue.IsEmpty()){
		bestaction = ac_queue.GetData(ac_queue.ActualHead());
	}
}

void Mediator::SetViewWidth(VIEWWIDTH view_width){
	if(view_width == VW_Narrow)
		DoLog(LOG_MEDIATION, "force narrow mode");
	else if(view_width == VW_Normal)
		DoLog(LOG_MEDIATION, "force normal mode");
	else
		DoLog(LOG_MEDIATION, "force wide mode");
	designated_viewwidth.Setdata(view_width, situation.CurrentTime);
}

bool Mediator::IsViewWidthSet(){
	return bool(designated_viewwidth.time == situation.CurrentTime);
}

void Mediator::SetViewRange(){
	float bdfy = Self.bodyfacing;//bdfy when exec this command

	if(IsViewWidthSet()){
		//当已经对期望视觉模式已经作出决策(不仅是确定), 直接利用下周期的视觉模式
		if(Self.NextViewCycle > situation.CurrentTime) {
			DoLog(LOG_SEE,"View2: %d",sensory.NextViewWidth);
			Validate_default_angle = Self.MyViewAngle(sensory.NextViewWidth); 
		}
		else{
			DoLog(LOG_SEE,"Time2: %d View2:%d",situation.CurrentTime,designated_viewwidth.data);
			Validate_default_angle = Self.MyViewAngle(designated_viewwidth.data);
		}
	}else{
		Validate_default_angle = Self.MyViewAngle(VW_Normal);
	}
	DoLog(LOG_VDEC, "view angle %.1f", Validate_default_angle);

	PrioredCommand bestaction;
	Getbestaction(bestaction);
	if (bestaction.type == CMD_turn){
		bdfy = NormalizeAngle(bestaction.angle * Skill::action.Turnrate() + bdfy);
	}
	Vang_inf = NormalizeAngle(bdfy - ServerParam::max_neck_angle - Validate_default_angle);
	Vang_sup = 2 * (ServerParam::max_neck_angle + Validate_default_angle);
	if(bestaction.type == CMD_none || bestaction.type == CMD_stay ){
		Vang_inf = NormalizeAngle(Vang_inf - Skill::action.Max_TurnAng());
		Vang_sup =  Vang_sup + 2 * Skill::action.Max_TurnAng();
	}
}

bool Mediator::ValidRequestAng(float ang, float shrinked_angle){
	return NormalizeAngle(ang - (Vang_inf + shrinked_angle) , 0) < Vang_sup - 2*shrinked_angle;
}

bool Mediator::enroll(float priority, AngleDeg angle){
	if(vr_queue.NumDatas() >= CP_max_visualrequests) return false;
	DoLog(LOG_MEDIATION, "enroll %.2f %.2f", priority, angle);
	return vr_queue.enqueue(VisualRequest(priority_mapping(Action_visual, priority), angle));
}

int Mediator::SetVisualRequests(VisualRequest * requests, int& totalrequests, VIEWWIDTH view_width, int max_requests){
	int p, q;
	float ang, priority, sum_priority, angle_inf, angle_sup, view_width_ang;
	VisualRequest treq;
	int max_idx = -1;
	float maxpriority = 0.0f;

	if (totalrequests < 0) return max_idx;
	if (vr_queue.IsEmpty()) return max_idx;

	priority = sum_priority = 0.0f;
	
	view_width_ang = Self.MyViewWidth(view_width);
	
	//当没有确定视觉模式时, 默认按VW_Normal的可达角度提交角度申请, 因此在VW_Narrow下需要检查
	bool validation = bool(designated_viewwidth.time != situation.CurrentTime &&  view_width == VW_Narrow);
	float shrinked_angle = 0;
	if(validation){
		shrinked_angle = Self.MyViewAngle(VW_Normal) - Self.MyViewAngle(view_width);
		if(shrinked_angle < 0) validation = false;
	}

	p = q = vr_queue.ActualHead();
	VisualRequest* pvr;
	if(validation){
		while(vr_queue.IsValid(p)){
			pvr = &vr_queue.GetData(p);
			pvr->valid = ValidRequestAng(pvr->angle, shrinked_angle);
			p = vr_queue.Next(p);
		}
		p = vr_queue.ActualHead();
	}
	//set visual-requests clockwisely
	while(vr_queue.IsValid(p)){
		if (totalrequests >= max_requests)//exceed max requests
			break;

		pvr = &vr_queue.GetData(p);
		if(validation && !pvr->valid){
			p = vr_queue.Next(p);
			continue;
		}
		requests[totalrequests].angle = pvr->angle;
		requests[totalrequests].view_width = view_width;
		requests[totalrequests].valid = true;

		if (sum_priority > 0){
			requests[totalrequests].priority = sum_priority
				= priority_sub(sum_priority, priority);
		}
		else{
			sum_priority = requests[totalrequests].priority = 0.0f;
		}

		if(p == q){
			requests[totalrequests].angle_inf = requests[totalrequests].angle;
		}
		else{
			requests[totalrequests].angle_inf = angle_inf;
		}
		
		pvr->view_width = view_width;
		priority = pvr->RequestedPriority();

		do{
			treq = vr_queue.GetData(q);
			treq.view_width = view_width;
			ang = NormalizeAngle(treq.angle - requests[totalrequests].angle, 0.0f);
			if (ang > view_width_ang){
				break;
			}			
			if(!validation || treq.valid){
				angle_inf = requests[totalrequests].angle_inf = treq.angle;
				sum_priority = requests[totalrequests].priority = 
					priority_sum(requests[totalrequests].priority, treq.RequestedPriority());
			}
			q = vr_queue.CircularNext(q);
		}while(q != p && vr_queue.IsValid(q));
		//when searching counterclockwisely, some requests will recur, seting angle_inf
		//will help to detect it
		pvr->angle_inf = angle_inf;

		if(requests[totalrequests].priority > maxpriority){
			maxpriority = requests[totalrequests].priority ;
			max_idx = totalrequests;
		}
		totalrequests ++;
		p = vr_queue.Next(p);
	}
//set visual-requests counter-clockwisely
	p = q = vr_queue.Tail();
	priority = sum_priority = 0.0f;
	int sup_element = -1;
	
	while(!vr_queue.IsHead(p) && vr_queue.IsValid(p)){
		if (totalrequests >= max_requests)//exceed max requests
			break;

		pvr = &vr_queue.GetData(p);
		if(validation && !pvr->valid){
			p = vr_queue.Prev(p);
			continue;
		}
		requests[totalrequests].angle_inf = pvr->angle;
		requests[totalrequests].view_width = view_width;
		requests[totalrequests].valid = true;

		if (sum_priority > 0){
			requests[totalrequests].priority = sum_priority 
				= priority_sub(sum_priority, priority);
		}
		else{
			sum_priority = requests[totalrequests].priority = 0.0f;
		}

		if(p == q){
			requests[totalrequests].angle = requests[totalrequests].angle_inf;
		}
		else{
			requests[totalrequests].angle = angle_sup;
		}

		pvr->view_width = view_width;
		priority = pvr->RequestedPriority();

		do{
			treq = vr_queue.GetData(q);
			treq.view_width = view_width;
			ang = NormalizeAngle(requests[totalrequests].angle_inf - treq.angle, 0.0f);
			if (ang > view_width_ang){
				break;
			}
			if(!validation || treq.valid){
				angle_sup = requests[totalrequests].angle = treq.angle;
				sum_priority = requests[totalrequests].priority = 
					priority_sum(requests[totalrequests].priority, treq.RequestedPriority());

				sup_element = q;
			}
			q = vr_queue.CircularPrev(q);
		}while(q != p && vr_queue.IsValid(q));