motion.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 <time.h>

#include "global.h"
#include "types.h"
#include "params.h"
#include "Command.h"

/**********************   Class Behaviors *********************************/
Behaviors::Behaviors(){
	Pass::Pass();
	Interception::Interception();
	VisualSystem::VisualSystem();
	AuditoryProcess::AuditoryProcess();
	Dribble::Dribble();
	Shoot::Shoot();
	Handleball::Handleball();
}

void Behaviors::Reset(){
	AuditoryProcess::ResetBuffer();
}

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

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

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

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

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

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

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

	num = motion.Num_TheirVisiblePlayers();
	for(i =0; i < num; i++){
		if (motion.TheirPlayer_Close2Me(i).pos_conf < 0.8f) continue;
		if (selfpredictpos.dist2(motion.TheirPlayer_Close2Me(i).pos) <
			Sqr(SP_player_size + SP_player_size)){
			if (marchway.HalfPlaneTest(motion.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(SP_ball_size + SP_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)){
		motion.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, action.Min_TurnAng(), 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);
	mediator.enroll(command, Action_other, PriorityA);
	return angle;
}

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

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

bool Behaviors::smartturn(AngleDeg ang){
	Command command;
	bool success = smartturn(ang, command);
	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, SP_min_neck_angle, SP_max_neck_angle);
	ang = ang - Self.head_angle;
	pose_limitation(ang, SP_min_neck_moment, SP_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);
	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) / action.Dashrate();
	else
		dashpow = (rapidness + Self.speed) / action.Dashrate();
	//caution of dashing backward
	pose_limitation(dashpow, SP_min_power, SP_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);
	mediator.enroll(command, Action_other, PriorityA);
	return success;
}

bool Behaviors::run(float rapidness){
	Command command;
	run(rapidness, command);
	mediator.enroll(command, Action_other, PriorityA);
	return true;
}

bool Behaviors::seize(){
	Command command;
	bool success = seize(command);
	mediator.enroll(command, Action_catch, PriorityA);
	return success;
}

bool Behaviors::seize(Command& command){
	if (!ball.catchable() || !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);
	mediator.enroll(command, Action_other, PriorityA);
	return success;
}

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


bool Behaviors::within_range(Vector& pos){
	return bool((Self.pos - pos).mod2() < 3);
}

bool Behaviors::infield(Vector pos){
	return bool(fabs(pos.x) <= SP_pitch_length/2 + 0.2f && fabs(pos.y) <= SP_pitch_width/2 + 0.2f);
}

bool Behaviors::incourt(Vector pos){
	return bool(fabs(pos.x) <= SP_pitch_length/2 + SP_pitch_margin &&
		fabs(pos.y) <= SP_pitch_width / 2 + SP_pitch_margin);
}


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*SP_ball_size+SP_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;
			}
				/*vecdist=myway.dist(ball.pos);
			if(vecdist<=SP_ball_size+SP_player_size){
				project=myway.ProjectPoint(ball.pos);
				if(myway.IsPtCloserToPtOnLine(ball.pos,nextpos,pos)){
					//确信球堵住了我的路
					float tangent=ASin((SP_ball_size+SP_player_size)/ball.distance)+10;
					Vector balltome=ball.pos-nextpos;
					Vector tartome=pos-nextpos;
					if(NormalizeAngle(balltome.Angle()-tartome.Angle())>0){
						tartome=tartome.Rotate(-tangent);
					}else	tartome=tartome.Rotate(tangent);
					Vector newtar=nextpos+tartome;
					my_error("(%4.2f,%4.2f)-(%4.2f,%4.2f)",Self.pos.x,Self.pos.y,newtar.x,newtar.y);
					my_error("nextpos(%4.2f,%4.2f),bodyfacing-%4.2f,speed-%4.2f",nextpos.x,nextpos.y,Self.bodyfacing,Self.speed);
					return go_to(newtar,speed,command);
				}
			}*/
		}
		if (Num_MyVisiblePlayers()>0&&MyPlayer_Close2Me(0).distance<checkmindis)
		{
			if (mindis>MyPlayer_Close2Me(0).distance) mindis = MyPlayer_Close2Me(0).distance;
			if (!MyPlayer_Close2Me(0).speed_valid())
			crossvel = 0;
			else
				crossvel = float(fabs(MyPlayer_Close2Me(0).global_vel.Rotate(-(pos-Self.pos).Angle()).y));
				blockrate=2.0f*SP_player_size/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(Num_TheirVisiblePlayers()>0&&TheirPlayer_Close2Me(0).distance<checkmindis){
			if (mindis>TheirPlayer_Close2Me(0).distance) mindis=TheirPlayer_Close2Me(0).distance;
			if (!TheirPlayer_Close2Me(0).speed_valid())
			crossvel = 0;
			else
				crossvel = float(fabs(TheirPlayer_Close2Me(0).global_vel.Rotate(-(pos-Self.pos).Angle()).y));
			blockrate=2.0f*SP_player_size/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*SP_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>SP_semi_pitch_length + SP_pitch_margin )
			newpos.x=SP_semi_pitch_length + SP_pitch_margin ;
		if (newpos.x<-(SP_semi_pitch_length + SP_pitch_margin ))
			newpos.x=-(SP_semi_pitch_length + SP_pitch_margin );
		if (newpos.y>SP_semi_pitch_width + SP_pitch_margin )
			newpos.x=SP_semi_pitch_width + SP_pitch_margin ;
		if (newpos.y<-(SP_semi_pitch_width + SP_pitch_margin ))
			newpos.y=-(SP_semi_pitch_width + SP_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);
	mediator.enroll(command, Action_positioning_setplay, PriorityA);
	return success;
}