📄 decision.cpp
字号:
// Decision.cpp: implementation of the CDecision class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "Robocup.h"
#include "Decision.h"
#include "Object.h"
#include "math.h"
#include "stdlib.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
extern Vector m_OpponentGoal;
int TheLest;
//周全——2003年6月27日19:00
//注意，机器人是0--4号；现假设0号为守门员已经确定，决策只是在1、2、3、4里面进行
//所有数字的单位都是厘米，场地大小为:290*240(cm*cm) 角度在-180——+180之间
//考虑用两个函数分别作出进攻和防守的决策，
//另外设置一个整体函数进行集成，1、判断整体态势2、角色选择3、角色动作和目标点的选择
//动作中直接将命令发送出去,每个机器人的动作和目标点判定出来以后直接发出命令动作（通讯部分）
//以上为方案1
CDecision::CDecision()
{
/* MAttacker=1;
AAssist1=2;
AAssist2=3;
AttackFreeMan=4;
MainDeffenser1=1;
MainDeffenser2=2;
DeffenseAssistant=3;
DeffenseFreeMan=4;
*/
}
CDecision::~CDecision()
{
}
//简单决策：
/*
void CDecision::SimpleDecision()
{
for(int i=0; i<5;i++)
{
MainAttacker=i;
if( g_MyTeam[MainAttacker].Get_vPos().x>=g_Ball.Get_vPos().x)
{
if ( g_MyTeam[MainAttacker].Get_vPos().y>120)
// {
if( (g_MyTeam[MainAttacker].Get_m_Angle()<=90)&&((g_MyTeam[MainAttacker].Get_m_Angle()>=0)))
{
g_MyTeam[MainAttacker].BasicAct(0,FALSE,TRUE,-MAX_VELOCITY);
}
if( (g_MyTeam[MainAttacker].Get_m_Angle()<=180)&&((g_MyTeam[MainAttacker].Get_m_Angle()>=90)))
{
g_MyTeam[MainAttacker].BasicAct(0,FALSE,TRUE,MAX_VELOCITY);
}
if( (g_MyTeam[MainAttacker].Get_m_Angle()<=0)&&((g_MyTeam[MainAttacker].Get_m_Angle()>=-90)))
{
g_MyTeam[MainAttacker].BasicAct(0,FALSE,TRUE,-MAX_VELOCITY);
}
if( (g_MyTeam[MainAttacker].Get_m_Angle()<=0)&&((g_MyTeam[MainAttacker].Get_m_Angle()>=90)))
{
g_MyTeam[MainAttacker].BasicAct(0,FALSE,TRUE,MAX_VELOCITY);
}
}
}
else
{
if( DistRandB(g_MyTeam[MainAttacker],g_Ball)<SHOOTDIST )
{
if( currentBall.Get_vPos().y<=m_OpponentGoal.y )
{
g_MyTeam[MainAttacker].BasicAct(10,TRUE,FALSE,MAX_VELOCITY);///Lshoot执行射门动作：左射门；
}
else
{
g_MyTeam[MainAttacker].BasicAct(10,FALSE,FALSE,MAX_VELOCITY);// Rshoot;
}
}
else
{
if(fabs(k1-k2)<PI/4)
{
g_MyTeam[MainAttacker].BasicAct(0,FALSE,TRUE,MAX_VELOCITY);
}
else
{
g_MyTeam[MainAttacker].TurnTo4Debug(g_Ball.Get_vPos());
}
}
}
MainAttackA();
}
*/
void CDecision::PredictBall()//对从视觉送来的每一帧都做关于球的处理；
{
currentBall=g_Ball;
double dx=currentBall.Get_vPos().x-lastBall.Get_vPos().x;
double dy=currentBall.Get_vPos().y-lastBall.Get_vPos().y;
predictedBall.m_vPos.x=currentBall.Get_vPos().x+dx;
predictedBall.m_vPos.y=currentBall.Get_vPos().y+dy;
lastBall.m_vPos.x=currentBall.Get_vPos().x;
lastBall.m_vPos.y=currentBall.Get_vPos().y;
}
/*
double CDecision::DistVandV(Vector &vector1, Vector &vector2)
//求两点之间的距离的平方
{
double dx,dy,dz;
dx=(vector1.x-vector2.x);
dy=(vector1.y-vector2.y);
dz=dx*dx+dy*dy;
return dz;
}
double CDecision::DistRandB(Player &player, Ball &ball)//求robot与ball之间的距离
{
return DistVandV( player.Get_vPos(), ball.Get_vPos() );
}
double CDecision::DistRandR(Player &player1, Player &player2)
{
return DistVandV( player1.Get_vPos(), player2.Get_vPos() );
}
*/
/*int CDecision::IsHaveObstacle(Vector &vector1, Vector &vector2)
//判断两点之间是否有障碍物的函数，真则返回TRUE，否则返回FALSE；
//算法为：建立两点之间的连线，如果其他机器人到该直线的距离小于某个值时，则说明该机器人为障碍；
{
//vector1=(x1,y1),vector2=(x2,y2) 可以得到直线方程（y2-y1)*x-(x2-x1)*y-(x2-y2)*(x2-x1)=0
for(int i=0;i<7;i++)
{
if(x[i]<x2&&x[i]>x1&&yi<y2&&yi>y1)
{
if(abs((y2-y1)*xi-(x2-x1)*yi-(x2-y2)*(x2-x1))/sqrt((y2-y1)*(y2-y1)+(x2-x1)+(x2-x1))<DMin) //角度和距离的极小值参数
{
return i; //返回该球员的号码
}
return -1;
}
}
return -1;
}
int CDecision::ChooseLest(double *array)
{
double *p,lest;
int i;
if( array!=NULL)
{
p=array;
}
i=0;
lest=*p;
for ( ; p!=NULL; p++ )
{
if ( *p<lest )
{
lest=*p;
i++;
}
}
return i;//返回数组中值最小的标号；
}
int CDecision::WhoHasBall()//我们控球则返回队员的标号，且其为主要进攻者
{
static double dist[11];//全局变量，用来存储10个机器人与球的距离
dist[0]=10000.0;//令dist[0]为较大值，有利于返回队员的标号
for ( int i=1; i<6; i++ )//将我方robot与球的距离赋给数组dist[1，2，3，4，5];
{
dist[i]=DistRandB( g_MyTeam[i-1], currentBall );
}
for ( ; i<11; i++ )//将对方robot与球的距离赋给数组dist[6,7,8,9,10];
{
dist[i]=DistRandB(g_TheirTeam[i], currentBall );
}
//从dist[]中找出最小的一个，如果他们的队员离球最近，则返回其队员标号；
TheLest=ChooseLest(dist);//从dist[]中选出最小的；
return TheLest;
}
state CDecision::stateDecision()
{
if ( ( (TheLest>0) && ( TheLest<6) ) && (g_MyTeam[TheLest].Get_vPos().x<currentBall.Get_vPos().x) && ( fabs( tn(TheLest)-g_MyTeam[TheLest].Get_m_Angle() ) )<(PI/3) )
//如果我方与球距离最小的其x位置比球
//小既靠近我方半场，且方向与球和机器人的连线的偏差不超过一个定值PI/3时，则说我方控球
{
return ATTACK;
}
return DEFFENSE;
}
int CDecision::AAssistant1()//以下为1号辅攻队员的选取，最靠近前方的球员（除了主攻者）动作就是进攻跑位
//确定第一辅助进攻者并返回其标号；
{
int k=0;
double l=0;
for( int i=1; i<5; i++ )
{
if(i==MAttacker)
{
continue;
}
if( g_MyTeam[i].Get_vPos().x>l )
{
k=i;
l=g_MyTeam[i].Get_vPos().x;
}
}
return k;
}
int CDecision::AAssistant2()//以下为2号辅攻队员的选取：除了MainAttacker,Assistant1以外的最靠近前方的球员；目的：位置在带球人的后面，
//动作顺序为：判断是否满足了条件，（位置在带球队员的身后的某个区域）不满足，则运动到所定义的点：
//满足条件则保持队形，根据带球队员的位置来调整相应的位置（确定目标点）
{
int k=0;
double l=0;
for( int i=0; i<5; i++ )
{
if( (i==MAttacker)||(i==AAssist1) )
{
continue;
}
if( g_MyTeam[i].Get_vPos().x>l )
{
k=i;
l=g_MyTeam[i].Get_vPos().x;
}
}
return k;
}
int CDecision::CAFreeMan()//自由人的选取
//判断出前三个之后剩下的即为自由人
{
int k=0;
for ( int i=1; i<5; i++ )
{
if ( ( i!=MAttacker )&&( i!=AAssist1 )&&( i!=AAssist2 ) )
{
k=i;
break;
}
}
return i;
}
void CDecision::MainAttackA()//6.27机器人能找到球，并且运动到适当位置（球的后面），进行粗糙的射门
{
// MAttacker=WhoHasBall();
double k1,k2,k3;
// int m;
k1=g_MyTeam[MAttacker].Get_m_Angle();
k2=(g_MyTeam[MAttacker].Get_vPos().y-currentBall.m_vPos.y)/( g_MyTeam[MAttacker].Get_vPos().x-currentBall.m_vPos.x );
k3=(g_MyTeam[MAttacker].Get_vPos().y-m_OpponentGoal.y)/( g_MyTeam[MAttacker].Get_vPos().x-m_OpponentGoal.x );
// double time=sqrt(currentBall.Get_vPos(),g_MyTeam[MAttacker].Get_vPos())*100;
double time=500;
if( ( g_MyTeam[MAttacker].Get_vPos().x<currentBall.m_vPos.x ) )// ( fabs(k3-k2)<(PI/6) )机器人在球的后面
{
if( DistRandB( g_MyTeam[MAttacker], currentBall )<SHOOTDIST ) //SHOOTDIST为射门距离，自定义
{
if( fabs(k1-k2)<(PI/3) )
{
if( currentBall.Get_vPos().y<=m_OpponentGoal.y )
{
g_MyTeam[MAttacker].BasicAct(10,TRUE,FALSE,MAX_VELOCITY,time);///Lshoot执行射门动作：左射门；
}
else
{
g_MyTeam[MAttacker].BasicAct(10,FALSE,FALSE,MAX_VELOCITY,time);// Rshoot;
}
}
else
{
g_MyTeam[MAttacker].TurnTo4Debug(g_Ball.Get_vPos());//DirectionProcess; 进行正向处理；
}
}
else
{
g_MyTeam[MAttacker].BasicAct(0,TRUE,TRUE,MAX_VELOCITY,time);
}
}
else//机器人在球的前面，先返回，就是做防守跑位:先简单作为跑到边线再回去
{
if( g_MyTeam[MAttacker].Get_vPos().y>120 )//从左边返回去
{
if( ( fabs(g_MyTeam[MAttacker].Get_m_Angle()-90)>10 )||( fabs(g_MyTeam[MAttacker].Get_m_Angle()+90)>10 ) )
{
Vector destPos=(g_MyTeam[MAttacker].Get_vPos().x, 160);
g_MyTeam[MAttacker].TurnTo(destPos);
}
else
{
g_MyTeam[MAttacker].BasicAct(0,0,1,MAX_VELOCITY,time);
}
}
}
}
*/
/*以下为传球判断： 不要了
m=Shooter();
if( m!=-1 ) // 判定前方是否有我方队员，适于传球用
{
//对应函数为：return sendball; //传球动作目标点为本方接球队员 动作即为sendball;
//在传球动作结束后 应break 将接球队员的角色转换为主攻队员
g_MyTeam[MAttacker].SendBall((g_MyTeam[MAttacker].Get_vPos()-g_MyTeam[m].Get_vPos()).mod(), g_MyTeam[m].Get_vPos());
}
else
{
//对应函数为：return dribble; //带球动作待定 其中应该包括正向处理动作
//带球动作中只考虑一个周期内的最优目标点 每个周期后都要重新判断
g_MyTeam[MAttacker].MoveTo(currentBall.Get_vPos());
}
*/
/*
int CDecision::Shooter() //计算一个我方前场队员的号码，并返回该值，如果我方前场没人，则返回-1;
{
int i=1,k;
for(;i<5;i++)
{
if(g_MyTeam[i].Get_vPos().x>=2933) {k=i;break;}
else k=-1;
}
return -1;
}
*/
/*
void CDecision::AssistAttack1A()
{
AAssist1=AAssistant1();
Vector dest=(g_MyTeam[AAssist1].Get_vPos().x,ShootP());
if( g_MyTeam[AAssist1].Get_vPos().x>g_MyTeam[MAttacker].Get_vPos().x )
//该辅助进攻者已经在主攻者前方，则选取最佳的进攻点，
//运动到该点（最好是正向处理的运动） 由ShootP()确定
{
// return moveto;or DirectionProcess;//对应函数为： //
g_MyTeam[AAssist1].MoveToWithAngle(dest,45); //假设45为对着对方球门的方向
}
else//在带球队员的后面的话，则运动到其前方，可以做好射门或者接传球的准备
{
// return moveto; //对应函数为：
Vector dest=(g_MyTeam[AAssist1].Get_vPos().x,ShootP());
g_MyTeam[AAssist1].MoveToWithAngleOA(dest,45);//假设45为对着对方球门的方向
}
}
int CDecision::ShootP()//找出最佳射门位置的纵坐标，并返回此值
{
double p1,p2,p,y;
if(currentBall.Get_vPos().x-g_TheirTeam[0].Get_vPos().x)//g_TheirTeam[0]为对方守门员
{
y=(4000-currentBall.Get_vPos().x)*(currentBall.Get_vPos().y-g_TheirTeam[0].Get_vPos().y)/(currentBall.Get_vPos().x-g_TheirTeam[0].Get_vPos().x)+currentBall.Get_vPos().y;
p1=fabs(atan2(currentBall.Get_vPos().y-1600,currentBall.Get_vPos().x-4000)-atan2(currentBall.Get_vPos().y-g_TheirTeam[0].Get_vPos().y,currentBall.Get_vPos().x-g_TheirTeam[0].Get_vPos().x));
p2=fabs(atan2(currentBall.Get_vPos().y-900,currentBall.Get_vPos().x-4000)-atan2(currentBall.Get_vPos().y-g_TheirTeam[0].Get_vPos().y,currentBall.Get_vPos().x-g_TheirTeam[0].Get_vPos().x));
p=p1>p2?(1600-(1600-y)/3):(900+(y-900)/3);
}
else
p=1250;
return (int)p;
}
void CDecision::AssistAttack2A()//以下为第二辅助进攻队员的动作规划及其目标点的选取
{
AAssist2=AAssistant2();
Vector dest=(g_MyTeam[MAttacker].Get_vPos().x-40,g_MyTeam[MAttacker].Get_vPos().y );
if( g_MyTeam[AAssist2].Get_vPos().x<g_MyTeam[MAttacker].Get_vPos().x )
//如果在带球人后方，则只需要做正向处理运动到带球队员后方的某个点,根据带球队员的位置
//来确定，如：其中400为经验值
{
//对应函数为return moveto;：
g_MyTeam[AAssist2].MoveTo(dest);
}
//防守跑位动作，同样可用于对自己控球队员的接应； return DPosition;
//对应函数：
g_MyTeam[AAssist2].MoveToWithAngle(dest,0);
}
void CDecision::AFreeManA()//FreeMan动作规划和目标点的选取，
{
AttackFreeMan=CAFreeMan();
double k2,k3;
k2=(g_MyTeam[MAttacker].Get_vPos().y-currentBall.m_vPos.y)/( g_MyTeam[MAttacker].Get_vPos().x-currentBall.m_vPos.x );
k3=(g_MyTeam[MAttacker].Get_vPos().y-m_OpponentGoal.y)/( g_MyTeam[MAttacker].Get_vPos().x-m_OpponentGoal.x );
if( g_MyTeam[AttackFreeMan].Get_vPos().x>g_MyTeam[MAttacker].Get_vPos().x )//在带球队员前方则回防
{
// return DPosition;//目标点为：
Vector dest=(currentBall.Get_vPos().x, 30);//y为我方球门前某点，可以人为设定
g_MyTeam[AttackFreeMan].MoveToWithAngleOA(dest,180);
}
else//选择最有利于进攻的点
{
if( ( g_MyTeam[MAttacker].Get_vPos().x<currentBall.Get_vPos().x )&& ( fabs(k3-k2)<(PI/3) ) )
//SHOOTDIST为射门距离，自定义
//判断是否满足射门条件，比主攻者的条件宽松一点，但是不能撞我方的主攻者
//或者判断与球之间是否有障碍，没有的时候才进行该动作
{
//return moveto;// or shoot; //避障运行到球的坐标，随机的创造机会，目标为球
g_MyTeam[AttackFreeMan].MoveTo(currentBall.Get_vPos());//对应函数：
}
// 对应函数：return DPosition;//目标点为：y为我方球门前某点，可以人为设定
Vector dest=(currentBall.Get_vPos().x, 40);
g_MyTeam[AttackFreeMan].MoveToWithAngleOA(dest,180);
}
}
//2、防守时的角色分配
//MainDeffenser1, MainDeffenser2, DeffenseAssistant,DFreeMan
//两个主要防守者+一个辅助防守者+一个自由人
//两个主要防守者是球的我方半场中，离球最近的球员
int CDecision::MDeffenser1()
{
for( int i=1; i<5; i++ )//从场上四个机器人当中选取
{
if( (g_MyTeam[i].Get_vPos().x<currentBall.Get_vPos().x) )
{
if( fabs(g_MyTeam[i].Get_m_Angle()-tn(i) )<PI/4 ) return i;
//一般这个角色是一定能取到的
}
}
//没有合适条件（即没有机器人在球的后方）
return AttackFreeMan;//返回上次进攻决策中FreeMan的标号
}
int CDecision::MDeffenser2()
{
for ( int i=1; i<5; i++ )
{
if( i!=MainDeffenser1 )//不能和MainDeffenser1冲突
{
if( g_MyTeam[i].Get_vPos().x<g_TheirTeam[MAttacker].Get_vPos().x )
{//选择我方球员中球左边第一个队员，即横坐标小于球的横坐标但最小的那个人
if( fabs(tn(i)-g_MyTeam[i].Get_m_Angle())<(PI/4) ) return i;//可调整
}
}
}
return AAssist2;//返回上次进攻决策中AttackerAssistant2的标号
}
int CDecision::DAssistant()//辅助进攻者的选取
{
int k=0;
for( int i=1; i<5; i++ )
{
if( (i!=MainDeffenser1)&&(i!=MainDeffenser2) )
{
k=i;
break;
}
}
return k;
}
int CDecision::DFreeMan()
{
int k=0;
for( int i=1; i<5; i++ )
{
if( (i!=MainDeffenser1)&&(i!=MainDeffenser2)&&(i!=DeffenseAssistant) )
{
k=i;
break;
}
}
return i;
}
//防守时的动作规划及目标点选取
void CDecision::MDeffenser1A()
{
MainDeffenser1=MDeffenser1();
if( g_MyTeam[MainDeffenser1].Get_vPos().x<currentBall.Get_vPos().x )
{
//对应函数：return block;//目标点为predictedBall.pos
g_MyTeam[MainDeffenser1].MoveTo(g_Ball.Get_vPos());
}
//对应函数： return DPosition;//目标点为：(PredictedBall.x-100, PredictedBall.y)100为经验常数
g_MyTeam[MainDeffenser1].MoveToWithAngleOA(g_Ball.Get_vPos(),180);
}
void CDecision::MDeffenser2A()
{
MainDeffenser2=MDeffenser2();
if( g_MyTeam[MainDeffenser1].Get_vPos().x<currentBall.Get_vPos().x )
{
//return block;//目标点为
Vector dest=(predictedBall.Get_vPos().x-20,predictedBall.Get_vPos().y-20 );
g_MyTeam[MainDeffenser1].MoveTo(dest);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -