motion.cpp

2002年

#include <time.h>

#include "types.h"
#include "worldmodel.h"
#include "skill.h"
#include "perceptron.h"
#include "motion.h"
#include "agent.h"

/**********************   Class Behaviors *********************************/
//This class implements some simple sequential actions.

Behaviors::Behaviors(){
}

void Behaviors::Reset(){
}

bool Behaviors::go_to(Vector pos, float rapidness){
	Command command;
	bool success = go_to(pos, rapidness, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return success;
}

bool Behaviors::go_to(Vector pos, float rapidness, Command& command){
	command.Reset();
	if (!PitchInfo.WithInMarginField(pos))	return false; // not in the court
	
	Vector gap = pos - Self.PredictPos();
	float angle;
	if (gap.mod2() < Sqr(ClientParam::goto_allowed_error)) return false; // do not bother to adjust your position

	if (rapidness < 0)
		angle = NormalizeAngle(gap.Angle() + 180.0f - Self.bodyfacing);
	else
		angle = NormalizeAngle(gap.Angle() - Self.bodyfacing);

	if( fabs(angle) > 15 - 8*exp(-gap.mod() * 0.2f)){
		smartturn(angle, command);
	}
	else{
		run(rapidness, command);
	}
	return true;
}

bool Behaviors::look_goto(Vector pos, float look_angle, float visual_width, float rapidness, float reverse_raipdness){
	Command cmd;
	look_goto(pos, look_angle, visual_width, rapidness, reverse_raipdness, cmd);
	Skill::action.execute(cmd);
	return true;
}

bool Behaviors::look_goto(Vector pos, float look_angle, float visual_width, float rapidness, float reverse_raipdness, Command& command){
//	return go_to(pos, rapidness, command);
	command.Reset();
	if (!PitchInfo.WithInMarginField(pos))	return false; // not in the court

	Vector gap = pos - Self.PredictPos();
	if (gap.mod2() < Sqr(ClientParam::goto_allowed_error)) return false; // do not bother to adjust your position

	float angle = gap.Angle();
	float anglegap = NormalizeAngle(angle - look_angle);
	bool reverse = false;
	if(fabs(anglegap) > visual_width){
		reverse = true;
		angle = NormalizeAngle(angle + 180.0f - Self.bodyfacing);
	}else{
		angle = NormalizeAngle(angle - Self.bodyfacing);
	}
	if( fabs(angle) > 15 - 8*exp(-gap.mod() * 0.2f)){
		smartturn(angle, command);
	}else{
		run(reverse? -reverse_raipdness : rapidness, command);
	}
	return true;
}

bool Behaviors::circumambulate(Vector pos, float rapidness){
	Command command;
	bool success = go_to(pos, rapidness, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return success;
}

bool Behaviors::circumambulate(Vector pos, float rapidness, Command& command){
	command.Reset();
	
	if (!PitchInfo.WithInMarginField(pos))
		return false; // not in the court
	
	Vector gap = pos - Self.PredictPos();
	float angle = gap.Angle();
	if (gap.mod2() < Sqr(ClientParam::goto_allowed_error)) 
		return false; // do not bother to adjust your position

	Line marchway;
	Vector selfpredictpos;
	marchway.LineFromRay(Ray(Self.pos, angle));
	int num = FieldInfo.Num_MyVisiblePlayers();	
	int i;
	for(i =0; i < num; i++){
		if (FieldInfo.MyPlayer_Close2Me(i).pos_conf < 0.8f) continue;
		selfpredictpos = Self.pos + Polar2Vector(rapidness, angle);
		if (selfpredictpos.dist2(FieldInfo.MyPlayer_Close2Me(i).pos) <
			Sqr(ServerParam::player_size + ServerParam::player_size)){
			if (marchway.HalfPlaneTest(FieldInfo.MyPlayer_Close2Me(i).pos) == 0){
				angle = NormalizeAngle(angle + 45);
				break;
			}
			else{
				angle = NormalizeAngle(angle - 45);
				break;
			}
		}
	}

	num = FieldInfo.Num_TheirVisiblePlayers();
	for(i =0; i < num; i++){
		if (FieldInfo.TheirPlayer_Close2Me(i).pos_conf < 0.8f) continue;
		if (selfpredictpos.dist2(FieldInfo.TheirPlayer_Close2Me(i).pos) <
			Sqr(ServerParam::player_size + ServerParam::player_size)){
			if (marchway.HalfPlaneTest(FieldInfo.TheirPlayer_Close2Me(i).pos) == 0){
				angle = NormalizeAngle(angle + 45);
				break;
			}
			else{
				angle = NormalizeAngle(angle - 45);
				break;
			}
		}
	}

	if (ball.pos_conf > 0.8f){
		if (selfpredictpos.dist2(ball.pos) < Sqr(ServerParam::ball_size + ServerParam::player_size)){
			if (marchway.HalfPlaneTest(ball.pos)){
				angle = NormalizeAngle(angle + 30.0f);
			}
			else{
				angle = NormalizeAngle(angle - 30.0f);
			}
		}
	}

	if (rapidness < 0)
		angle = NormalizeAngle(angle + 180 - Self.bodyfacing);
	else
		angle = NormalizeAngle(angle - Self.bodyfacing);

	if( fabs(angle) > 10 - 5*exp(-gap.mod() * 0.2f)){
		smartturn(angle, command);
	}
	else{		
		run(rapidness, command);		
	}
	return true;
}

AngleDeg Behaviors::turn_to(AngleDeg ang, Command& command){
	command.Reset();
	ang = NormalizeAngle(ang - Self.bodyfacing);
	pose_limitation(ang, Skill::action.Min_TurnAng(), Skill::action.Max_TurnAng());
	smartturn(ang, command);
	
	return ang;
}

AngleDeg Behaviors::turn_to(Vector pos, Command& command){
	return turn_to((pos - Self.pos).Angle(), command);
}

AngleDeg Behaviors::turn_to(Vector pos){
	Command command;
	AngleDeg angle = turn_to(pos, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return angle;
}

AngleDeg Behaviors::turn_to(AngleDeg ang){
	Command command;
	AngleDeg angle = turn_to(ang, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return angle;
}

bool Behaviors::smartturn(AngleDeg ang, Command& command){
	ang = ang / Skill::action.Turnrate();
	pose_limitation(ang, ServerParam::min_moment, ServerParam::max_moment);
	command.turn(ang);
	return true;
}

bool Behaviors::smartturn(AngleDeg ang){
	Command command;
	bool success = smartturn(ang, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return success;
}

AngleDeg Behaviors::turn_neck_to(AngleDeg ang, AngleDeg bdfy, Command& command){
	command.Reset();
	ang = NormalizeAngle(ang - bdfy);
	pose_limitation(ang, ServerParam::min_neck_angle, ServerParam::max_neck_angle);
	ang = ang - Self.head_angle;
	pose_limitation(ang, ServerParam::min_neck_moment, ServerParam::max_neck_moment);
	command.turn_neck(ang);
	return ang;
}

AngleDeg Behaviors::turn_neck_to(AngleDeg ang, Command& command){
	return turn_neck_to(ang, Self.bodyfacing, command);
}

AngleDeg Behaviors::turn_neck_to(AngleDeg ang){
	Command command;
	float angle = turn_neck_to(ang, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return angle;
}

bool Behaviors::run(float rapidness, Command& command){	
	float dashpow;
	if (fabs(NormalizeAngle(Self.vel_angle - Self.bodyfacing))<90)
		dashpow = (rapidness - Self.speed) / Skill::action.Dashrate();
	else
		dashpow = (rapidness + Self.speed) / Skill::action.Dashrate();
	//caution of dashing backward
	pose_limitation(dashpow, ServerParam::min_power, ServerParam::max_power);
	command.dash(dashpow);
	return true;
}

bool Behaviors::stay(Command&command){
	command.stay();
	return true;
}

bool Behaviors::stay(){
	Command command;
	bool success = stay(command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return success;
}

bool Behaviors::run(float rapidness){
	Command command;
	run(rapidness, command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return true;
}
/*
bool Behaviors::seize(){
	Command command;
	bool success = seize(command);
	Agent::mediator.enroll(command, Action_catch, PriorityA);
	return success;
}

bool Behaviors::seize(Command& command){
	if (!ball.catchable() || !Skill::action.cancatch(situation.CurrentTime)) return false;
	command.seize(NormalizeAngle(ball.global_angle - Self.bodyfacing));
	return true;
}*/

bool Behaviors::donothing(){
	Command command;
	bool success = donothing(command);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return success;
}

bool Behaviors::donothing(Command& command){
	command.donothing();
	return true;
}


bool Behaviors::smartgoto(Vector pos ,float speed,Command& command){
	return circumambulate(pos,speed,command);

	int i,j,degmin,degmax;
	int turncnt;
	bool AngleBlocked[24];
//每15度为一个方向，标记该方向是否畅通。
	Vector SelfPred=Self.PredictPos();
	float checkmindis=2.0f+speed;
//只考虑checkmindis范围内的障碍物
	float mindis=checkmindis;
//障碍物离自己的最小距离
	int go_through_angle;
	float targetangle=(pos-SelfPred).Angle();
	float crossvel;
//障碍物横穿目标路径速度。若横穿速度较大则不必考虑
    float dAngle;
	float blockrate;
	bool blocked;
//	float vecdist;
//	Vector project;
	Vector newpos;
		for (i=0;i<24;i++) AngleBlocked[i]=false; 
		//标记哪些方向上没有障碍
		if(situation.playmode!=PM_Play_On&&ball.pos_conf>=0.95f && ball.distance<checkmindis)
		{
			if (mindis>ball.distance) mindis=ball.distance;
			blockrate=(2*ServerParam::ball_size+ServerParam::player_size)/ball.distance;
			if (blockrate>1) blockrate=1;
			dAngle=ASin(blockrate);
			degmin=int((NormalizeAngle((ball.pos-SelfPred).Angle() - targetangle+7.5f)-dAngle)/15.0f+12); 
			degmax=int((NormalizeAngle((ball.pos-SelfPred).Angle() - targetangle+7.5f)+dAngle)/15.0f+12); 
			for (j=degmin;j<degmax+1;j++)
			{
				 AngleBlocked[(j+24)%24]=true;
			}
		}
		if(FieldInfo.Num_MyVisiblePlayers()>0 && FieldInfo.MyPlayer_Close2Me(0).distance<checkmindis)
		{
			if (mindis>FieldInfo.MyPlayer_Close2Me(0).distance) mindis = FieldInfo.MyPlayer_Close2Me(0).distance;
			if (!FieldInfo.MyPlayer_Close2Me(0).speed_valid())
			crossvel = 0;
			else
				crossvel = float(fabs(FieldInfo.MyPlayer_Close2Me(0).global_vel.Rotate(-(pos-Self.pos).Angle()).y));
				blockrate=2.0f*ServerParam::player_size/FieldInfo.MyPlayer_Close2Me(0).distance;
				if (blockrate>1) blockrate=1;
				dAngle=ASin(blockrate);
			//	LogAction("my player %d",i);
			if(crossvel<0.2)
			{
				 degmin=int((NormalizeAngle((MyPlayer(i).pos-SelfPred).Angle() - targetangle+7.5f)-dAngle)/15.0f+12); 
				 degmax=int((NormalizeAngle((MyPlayer(i).pos-SelfPred).Angle() - targetangle+7.5f)+dAngle)/15.0f+12); 
				for (j=degmin;j<degmax+1;j++)
				{
					//LogAction("blocked %d %d",j,j%24);
				 AngleBlocked[(j+24)%24]=true;
				}
			}
		}
		
    
		if(FieldInfo.Num_TheirVisiblePlayers()>0&&FieldInfo.TheirPlayer_Close2Me(0).distance<checkmindis){
			if (mindis>FieldInfo.TheirPlayer_Close2Me(0).distance) mindis=FieldInfo.TheirPlayer_Close2Me(0).distance;
			if (!FieldInfo.TheirPlayer_Close2Me(0).speed_valid())
			crossvel = 0;
			else
				crossvel = float(fabs(FieldInfo.TheirPlayer_Close2Me(0).global_vel.Rotate(-(pos-Self.pos).Angle()).y));
			blockrate=2.0f*ServerParam::player_size/FieldInfo.TheirPlayer_Close2Me(0).distance;
			if (blockrate>1) blockrate=1;
			dAngle=ASin(blockrate);
			if(crossvel<0.2){
				//LogAction("Their Player %d");
				 degmin=int((NormalizeAngle((TheirPlayer(i).pos-SelfPred).Angle() - targetangle+7.5f)-dAngle)/15.0f+12); 
				 degmax=int((NormalizeAngle((TheirPlayer(i).pos-SelfPred).Angle() - targetangle+7.5f)+dAngle)/15.0f+12); 
				for (j=degmin;j<degmax+1;j++)
				 AngleBlocked[(j+24)%24]=true;
				}
		}
		
		if (mindis>Self.pos.dist(pos)) 
		{
		//	LogAction("go to %f %f",pos.x,pos.y);
			return go_to(pos,speed,command);
		}
		//障碍物比目标距离远，不构成阻碍
		go_through_angle=int(2*ASin(1.6f*ServerParam::player_size/mindis)/15);
		//从障碍物间穿越所需的最小张角
		if (go_through_angle<1) go_through_angle=1;
		//寻找与目标方向最接近的可以通行的方向，即刚巧绕过障碍的方向
		degmin=degmax=12;
		for (turncnt=0;turncnt<24;turncnt++)
		{blocked =false;
			for (j=degmin;j>degmin-go_through_angle;j--)
				if (AngleBlocked[(j+24)%24]) 
				{blocked=true;
				 break;
				}
				if (blocked) degmin=(degmin+23)%24;
				else break;
				}
		for (turncnt=0;turncnt<24;turncnt++)
		{blocked =false;
			for (j=degmax;j<degmax+go_through_angle;j++)
				if (AngleBlocked[j%24]) 
				{blocked=true;
				 break;
				}
				if (blocked) degmax=(degmax+1)%24;
				else break;
		}
		degmin=(degmin-go_through_angle/2+24)%24;
		degmax=(degmax+go_through_angle/2)%24;
		//选需要转身的角度小的一个，保证下次仍然向同一方向运动
		i=(fabs(NormalizeAngle((degmin-12)*15+targetangle-Self.bodyfacing))<fabs(NormalizeAngle((degmax-12)*15+targetangle-Self.bodyfacing))?degmin-12:degmax-12);
		newpos = SelfPred+(pos-SelfPred).Rotate(i*15.0f);

		if (newpos.x>ServerParam::semi_pitch_length + ServerParam::pitch_margin )
			newpos.x=ServerParam::semi_pitch_length + ServerParam::pitch_margin ;
		if (newpos.x<-(ServerParam::semi_pitch_length + ServerParam::pitch_margin ))
			newpos.x=-(ServerParam::semi_pitch_length + ServerParam::pitch_margin );
		if (newpos.y>ServerParam::semi_pitch_width + ServerParam::pitch_margin )
			newpos.x=ServerParam::semi_pitch_width + ServerParam::pitch_margin ;
		if (newpos.y<-(ServerParam::semi_pitch_width + ServerParam::pitch_margin ))
			newpos.y=-(ServerParam::semi_pitch_width + ServerParam::pitch_margin );

	//	LogAction("pos %f %f",pos.x,pos.y);
	//	LogAction("turn angle %f",NormalizeAngle((newpos-SelfPred).Angle()-Self.bodyfacing));
	//	LogAction("go_to %f %f",newpos.x,newpos.y);
		return go_to(newpos,speed,command);
}

bool Behaviors::smartgoto(Vector pos ,float speed){
	Command command;
	bool success = smartgoto(pos,speed,command);
	Agent::mediator.enroll(command, Action_positioning_setplay, PriorityA);
	return success;
}