⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 strategy.cpp

📁 5V5仿真平台机器人足球
💻 CPP
📖 第 1 页 / 共 3 页
字号:
🔍
123 
					{
						if(zw.ball.pos.y>FMIDY)
						{
							assist_pos2.y=PenalT+Rob_adge;
							assist_pos2.x=FRIGHT+(PenalT+Rob_adge-FMIDY)*dpgx/dpgy;
						}
						else
						{
							assist_pos2.y=PenalB-Rob_adge;
							assist_pos2.x=FRIGHT+(PenalB-Rob_adge-FMIDY)*dpgx/dpgy;
						}
					}
					else
					{
						assist_pos2.x=PenalR-Rob_diam;
						assist_pos2.y=FMIDY-43*cm*dpgy/dpgx;
					}
				}
			}
			else
			{
				assist_pos2.x=PenalR-Rob_adge;
	
				if(zw.ball.pos.y>FMIDY)
					assist_pos2.y=PenalT;
				else
					assist_pos2.y=PenalB;
			}

			if((T_to_Bal<9*cm || Bal_to_B<9*cm) && zw.ball.pos.x>=FMIDX-60*cm && zw.ball.dx>2*cm)
			{
				if(zw.ball.pos.y>FMIDY)
				{
					assist_pos1.x=PenalR-11*cm;
					assist_pos1.y=GoalB-5*cm;
				}
				else
				{
					assist_pos1.x=PenalR-11*cm;
					assist_pos1.y=GoalT+5*cm;
				}
			}
			else if(zw.ball.pos.x<PenalR-35*cm)
			{
				assist_pos1.x=zw.ball.pos.x;
				if(assist_pos2.y<FMIDY)
					assist_pos1.y=assist_pos2.y+40*cm;
				else
					assist_pos1.y=assist_pos2.y-40*cm;
			}
			else
			{
				assist_pos1.x=PenalR-11*cm;
				if(zw.ball.pos.y<FMIDY)
					assist_pos1.y=GoalT+5*cm;
				else
					assist_pos1.y=GoalB-5*cm;
			}
		}
	}

	else// if(zw.ball.dx<0)
	{//球向右
		if(zw.ball.pos.x<PenalL+25*cm)
		{
			if(zw.ball.pos.x<FLEFT+10*cm)
			{
				assist_pos1.x=FLEFT+Rob_adge;
				assist_pos2.x=GoalL-Rob_rad40;
			}
			else if(zw.ball.pos.x<GoalL+Rob_adge)
			{
				assist_pos1.x=GoalL-2*cm;
				assist_pos2.x=GoalL;
			}
			else if(zw.ball.pos.x<PenalL)
			{
				assist_pos1.x=GoalL+Rob_adge;
				assist_pos2.x=GoalL+Rob_adge;
			}
			else
			{
				assist_pos1.x=PenalL;
				assist_pos2.x=PenalL+Rob_adge;
			}

			if(zw.ball.dy<-0.5*cm && zw.ball.pos.x>PenalB)
			{
				assist_pos1.y=PenalB+5*cm;
				assist_pos2.y=PenalT;
			}
			else if(zw.ball.dy>0.5*cm && zw.ball.pos.x<PenalT)
			{
				assist_pos1.y=PenalT-5*cm;
				assist_pos2.y=PenalB;
			}
			else
			{			
				if(zw.ball.pos.y<FMIDY)
				{
					assist_pos1.y=PenalT;
					assist_pos2.y=PenalB;
				}
				else
				{
					assist_pos1.y=PenalB;
					assist_pos2.y=PenalT;
				}
			}
		}
		else
		{
			if((T_to_Bal<9*cm || Bal_to_B<9*cm) && zw.ball.pos.x<FMIDX+40*cm && zw.ball.dx<-2*cm)
			{
				assist_pos1.x=FLEFT+55*cm;
				assist_pos2.x=zw.ball.pos.x;

				if(zw.ball.pos.y>FMIDY)
				{
					assist_pos1.y=PenalB-5*cm;
					assist_pos2.y=PenalT;
				}
				else
				{
					assist_pos1.y=PenalT+5*cm;
					assist_pos2.y=PenalB;
				}
			}
			else if(zw.ball.pos.x>PenalR-11*cm)
			{
				assist_pos1.x=PenalR-11*cm;
				assist_pos2.x=PenalR-Rob_adge;

				if(zw.ball.pos.y>FMIDY)
				{
					assist_pos2.y=PenalT;
					assist_pos1.y=GoalB-5*cm;
				}
				else
				{
					assist_pos2.y=PenalB;
					assist_pos1.y=GoalT+5*cm;
				}
			}
			else
			{
				assist_pos1.x=assist_pos2.x=zw.ball.prepos.x;
				if(zw.ball.prepos.y<FBOTTOM+45*cm)
				{
					assist_pos2.y=zw.ball.prepos.y+30*cm;
					assist_pos1.y=zw.ball.prepos.y+70*cm;
				}
				else if(zw.ball.prepos.y<FMIDY+45*cm)
				{
					assist_pos2.y=zw.ball.prepos.y-35*cm;
					assist_pos1.y=zw.ball.prepos.y+35*cm;
				}
				else
				{
					assist_pos2.y=zw.ball.prepos.y-30*cm;
					assist_pos1.y=zw.ball.prepos.y-70*cm;
				}
			}
		}
	}

	zw.assist1_pos.x=assist_pos1.x;
	zw.assist1_pos.y=assist_pos1.y;
	zw.assist2_pos.x=assist_pos2.x;
	zw.assist2_pos.y=assist_pos2.y;

}

//////////top: Determine_The_Assistant_Points()/////////////////////////////////
void Determine_Defend_Point()
{
	Vector2D defend_pos, def_pos;
	static double def_posY;

	double dpgx=zw.ball.prepos.x-FRIGHT;
	double dpgy=zw.ball.prepos.y-FMIDY;
	double dist_pg=sqrt(dpgx*dpgx+dpgy*dpgy);//球与己方底线门心交点
	double dpgtx=zw.ball.prepos.x-FRIGHT;
	double dpgty=zw.ball.prepos.y-PenalT-3*cm;
	double dist_pgt=sqrt(dpgtx*dpgtx+dpgty*dpgty);
	double dpgbx=zw.ball.prepos.x-FRIGHT;
	double dpgby=zw.ball.prepos.y-PenalB+3*cm;
	double dist_pgb=sqrt(dpgbx*dpgbx+dpgby*dpgby);
	double PGR_y, PGR_x, PFR_y;

	if(zw.ball.pos.x>FRIGHT-Rob_rad40 
		|| zw.ball.pos.x>PenalR-5*cm 
			&& zw.ball.pos.y>PenalB-3*cm && zw.ball.pos.y<PenalT+3*cm)
	{//球在右底线或在罚球区，防守队员在罚球区外等待
		defend_pos.x=PenalR-5*cm;
		if(zw.ball.pos.y>FMIDY)
			defend_pos.y=zw.ball.pos.y-15*cm;
		else
			defend_pos.y=zw.ball.pos.y+15*cm;
	}

	else if(zw.ball.pos.x>FMIDX)
	{//右半场站位
		if(zw.ball.pos.y<=PenalB-3*cm)
		{
			defend_pos.x=FRIGHT+dpgbx*0.6;
			defend_pos.y=PenalB-3*cm+dpgby*0.6;
		}
		else if(zw.ball.pos.y<=PenalT+3*cm)
		{
			defend_pos.x=FRIGHT+dpgx*0.6;
			defend_pos.y=FMIDY+dpgy*0.6;
		}
		else
		{
			defend_pos.x=FRIGHT+dpgtx*0.6;
			defend_pos.y=PenalT+3*cm+dpgty*0.6;
		}

		if(zw.ball.dx>0)
		{//球右移
			PFR_y=zw.ball.pos.y-(zw.ball.pos.x-FRIGHT)*zw.ball.dy/zw.ball.dx;

			if(PFR_y<=GateT && PFR_y>=GateB)
			{//若球冲门来,则站在球的前进线上
				dpgx=zw.ball.prepos.x-FRIGHT;
				dpgy=zw.ball.prepos.y-PFR_y;

				defend_pos.x=FRIGHT+dpgx*0.6;
				defend_pos.y=PFR_y+dpgy*0.6;
			}
		}

		if(defend_pos.x>PenalR-5*cm && defend_pos.y>PenalB-3*cm && defend_pos.y<PenalT+3*cm)
		{
			PGR_y=zw.ball.pos.y-(zw.ball.pos.x-PenalR-5*cm)*dpgy/dpgx;
			if(PGR_y>=PenalB-3*cm && PGR_y<=PenalT+3*cm)
			{
				defend_pos.y=PGR_y;
				defend_pos.x=PenalR-5*cm;
			}
			else
			{
				if(zw.ball.prepos.y>PenalT+3*cm)
				{
					PGR_x=zw.ball.prepos.x-(zw.ball.prepos.y-PenalT+3*cm)*dpgx/dpgy;
					defend_pos.x=PGR_x;
					defend_pos.y=PenalT+3*cm;
				}
				else if(zw.ball.prepos.y<PenalB-3*cm)
				{
					PGR_x=zw.ball.prepos.x-(zw.ball.prepos.y-PenalB-3*cm)*dpgx/dpgy;
					defend_pos.x=PGR_x;
					defend_pos.y=PenalB-3*cm;
				}
			}
		}

		if(defend_pos.x>FRIGHT-Rob_rad40)
			defend_pos.x=FRIGHT-Rob_rad40;
	}

	else
	{//左半场
		if(zw.ball.dx>1.5*cm && zw.ball.pos.x<FLEFT+65*cm)
		{
			def_pos.x=FLEFT+60*cm;
			def_pos.y=(def_pos.x-zw.ball.pos.x)*zw.ball.dy/zw.ball.dx+zw.ball.pos.y;
			if(def_pos.y>=GoalB && def_pos.y<=GoalT)
			{
				defend_pos.x=def_pos.x;
				defend_pos.y=def_pos.y;
			}
		}

		else if(zw.ball.pos.x<PenalL-5*cm )
		{
			defend_pos.x=PenalL+35*cm;
			if(zw.ball.pos.y<PenalB-5*cm || zw.ball.pos.y>PenalT+5*cm)
			{
				dpgx=zw.ball.pos.x-FMIDX;
				dpgy=zw.ball.pos.y-FMIDY;
				dist_pg=sqrt(dpgx*dpgx+dpgy*dpgy);

				defend_pos.y=zw.ball.pos.y-(zw.ball.pos.x-PenalL-35*cm)*dpgy/dpgx;
			}
			else
				defend_pos.y=FMIDY;
		}
		else
		{
			defend_pos.x=zw.ball.pos.x-40*cm*dpgx/dist_pg;
			defend_pos.y=zw.ball.pos.y-40*cm*dpgy/dist_pg;
		}
	}


	zw.defend_pos.x=defend_pos.x;
	zw.defend_pos.y=defend_pos.y;

}
/////////////////Top :Determine_Defend_Point()///////////////////////////////////////////////

void Defend_Act(int which, Vector2D point, Environment *env)
{
	PositAvoidBlock(which, point, env);
}
/////////////////Top :Defend_Act(int which, Envionment *env)///////////////////////////////////////////////

void Assign_Tasks_To_Team_Members(Environment *env)
{
	double dprx, dpry, distance_pr, minidistance=10000.0;
	int id;

	for(id=1; id<5; id++)
	{
		dprx=zw.kickPit.x-zw.home[id].pos.x;
		dpry=zw.kickPit.y-zw.home[id].pos.y;
		distance_pr=sqrt(dprx*dprx+dpry*dpry);
		if(distance_pr<minidistance)
		{
			minidistance=distance_pr;
			zw.attacker=id;
		}
	}

	minidistance=10000.0;
	for(id=1; id<5; id++)
	{
		if(id==zw.attacker) continue;

		dprx=zw.defend_pos.x-zw.home[id].pos.x;
		dpry=zw.defend_pos.y-zw.home[id].pos.y;
		distance_pr=sqrt(dprx*dprx+dpry*dpry);

		if(distance_pr<minidistance)
		{
			minidistance=distance_pr;
			zw.defender=id;
		}
	}
	
	minidistance=10000.0;
	for(id=1; id<5; id++)
	{
		if(id==zw.attacker || id==zw.defender) continue;
		dprx=zw.assist1_pos.x-zw.home[id].pos.x;
		dpry=zw.assist1_pos.y-zw.home[id].pos.y;
		distance_pr=sqrt(dprx*dprx+dpry*dpry);

		if(distance_pr<minidistance)
		{
			minidistance=distance_pr;
			zw.assist1=id;
		}
	}

	for(id=1; id<5; id++)
	{
		if(id!=zw.attacker && id!=zw.assist1 && id!=zw.defender)
		{
			zw.assist2=id;
			break;
		}
	}

	zw.goalie=0;
}
/////////////Top: Defend_Act(int which, Envionment *env)//////////////////

//void Attack_Act(int which, Environment *env)
//{
//	AttackAvoidBlock(which, point,  env);
//}
/////////////////////////////////////////////////////////////////

void Assistant_Act(int which, Vector2D point, Environment *env)
{	
	PositAvoidBlock(which, point, env);
}
//////////////Top: Assistant_Act(int which, Vector2D point, Environment *env)///////

///////////////////////////////////////////////////////////////
void Goalie_Act(int which, Environment *env)
{
	Vector2D locate;
	double dbgx, bal_gat_y, Dx=5*cm;

	double dbrx=zw.ball.pos.x-zw.home[which].pos.x;
	double dbry=zw.ball.pos.y-zw.home[which].pos.y;
	double dist_br=sqrt(dbrx*dbrx+dbry*dbry);//守门员与球的距离

	//当球向右运动时，计算球前进线与右底线交点纵坐标bal_gat_y
	if(zw.ball.dx>0)
	{
		dbgx=zw.ball.pos.x-FRIGHT;
		bal_gat_y=zw.ball.pos.y-dbgx*zw.ball.dy/zw.ball.dx;
	}

	if(zw.ball.dx>0 && bal_gat_y<=GateT+1*cm && bal_gat_y>=GateB-1*cm)
	{//如果球冲向球门
		if(zw.home[which].pos.x-zw.ball.pos.x>=Dist_RB)
		{//若球没与守门员接触,则站在门前球的前进线上
			locate.x=FRIGHT-Dx;
			locate.y=bal_gat_y-(Dx+Dist_RB)*zw.ball.dy/zw.ball.dx;
		}
		else
		{//若守门员接触球使自己角度或位置变化（可能使球滑向球门），则推球
			locate.x=FRIGHT-Dx;
			locate.y=zw.ball.pos.y;
		}

		if(locate.y>GoalT+4*cm)
			locate.y=GoalT+4*cm;
		if(locate.y<GoalB-4*cm)
			locate.y=GoalB-4*cm;
	}
	else if(zw.ball.pos.x>PenalR+10*cm)
	{//若球未冲向球门，且球到底线横向距离小于25cm
		locate.x=FRIGHT-Dx;
		locate.y=zw.ball.pos.y;//守门员站在球的水平线上

		if(zw.ball.pos.x<=zw.home[which].pos.x-Dist_RB)
		{//如果球在守门员左侧外，守门员不越过门柱
			if(locate.y>GateT)
				locate.y=GateT;
			if(locate.y<GateB)
				locate.y=GateB;
		}
		else
		{//否则，推球至球门区边界
			if(locate.y>GoalT+4*cm)
				locate.y=GoalT+4*cm;
			if(locate.y<GoalB-4*cm)
				locate.y=GoalB-4*cm;
		}
	}
	else
	{//若球到底线横向距离大于25cm，且球冲向球门两侧来或远离球门  
		double dbhx=zw.ball.pos.x-(FRIGHT+8);
		double dbhy=zw.ball.pos.y-FMIDY;

		locate.x=FRIGHT-Dx;
		locate.y=zw.ball.pos.y+(FRIGHT-Dx-zw.ball.pos.x)*dbhy/dbhx;
		if(locate.y>GoalT+2*cm)
			locate.y=GoalT+2*cm;
		if(locate.y<GoalB-2*cm)
			locate.y=GoalB-2*cm;
	}

	GoaliePosit(which, locate, env);//***
}
////////////////////////////////////////////////////////////////////////////////////////

/////////
void AttackAvoidBlock(int which, Vector2D point, Environment *env)
{
	int id,Block_id=-1;
	char block_direction;
	double mindis=1000.0, mind, n=11, distt=0,juli;
	Vector2D  robot, avoidpoint;
	double dphx=point.x-zw.home[which].pos.x;
	double dphy=point.y-zw.home[which].pos.y;
	double distance_ph=sqrt(dphx*dphx+ dphy*dphy);
	double dprx, dpry, distance_pr, drhx, drhy, distance_rh;
	double dahx,dahy,distance_ah;

	for(id=0;id<n;id++)
	{
		if(which==id) continue;

		if(id<5)
		{
			robot=zw.home[id].pos;
			mind=R10;
123

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -