intercept.cpp

浙江大学 RoboCup3D 2006 源代码

/***************************************************************************
 *   Copyright (C) 2004 - 2006 by ZJUBase                                  *
 *                                National Lab of Industrial Control Tech. *
 *                                Zhejiang University, China               *
 *                                                                         *
 *   Achievements of ZJUBase in RoboCup Soccer 3D Simulation League:       *
 *    In RoboCup championship,                                             *
 *      - June 2006 - 3rd place in RoboCup 2006, Bremen, Germany (lost     *
 *        the semi-final with a coin toss)                                 *
 *      - July 2005 - 3rd place in RoboCup 2005, Osaka, Japan (share the   *
 *        3rd place with team Caspian)                                     *
 *    In RoboCup local events,                                             *
 *      - April 2006 - 2nd place in RoboCup Iran Open 2006, Tehran, Iran   *
 *        (lost the final with a coin toss)                                *
 *      - December 2005 - 2nd place in AI Games 2005, Isfahan, Iran        *
 *      - July 2005 - champion in China Robot Competition 2005, Changzhou, *
 *        China                                                            *
 *      - October 2004 - champion in China Soccer Robot Competition 2004,  *
 *        Guangzhou, China                                                 *

*                                                                         *
 *   Team members:                                                         *
 *    Currently the team leader is,                                        *
 *           Hao JIANG ([e-mail]; [e-mail])       *
 *    In the next season, the leader will be                               *
 *           Yifeng ZHANG ([e-mail])                        *
 *    ZJUBase 3D agent is created by                                       *
 *           Dijun LUO ([e-mail])                             *
 *    All the members who has ever contributed:                            *
 *           Jun JIANG                                                     *
 *           Xinfeng DU ([e-mail])                             *
 *           Yang ZHOU ([e-mail])                             *
 *           Zhipeng YANG                                                  *
 *           Xiang FAN                                                     *
 *                                                                         *
 *   Team Manager:                                                          *
 *      Ms. Rong XIONG ([e-mail])                            *
 *                                                                         *
 *   If you met any problems or you have something to discuss about        *
 *   ZJUBase. Please feel free to contact us through EMails given below.   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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 General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "Intercept.h"

SoccerCommand Intercept::interceptFastest(Vector3 target, AgentStatus a, AgentStatus b)
{
	Vector3 dest;
	return interceptFastest(target, a, b, dest);
}

SoccerCommand Intercept::interceptFastest(Vector3 target,
										  AgentStatus a, AgentStatus b,
										  Vector3& dest)
{
	int i;
	AgentStatus ball_s;	
	double r,desired_ang;
	Vector3 dd;
	Vector3 p;
	r = 0.330;
	if (global.wm.pm == PM_KickIn_Our)
		r = 0.330;

	Line l;
	l=l.makeLineFromPositionAndAngle(Vector3(global.wm.ballPos.x,global.wm.ballPos.y),global.wm.ballVel.ang());
	Vector3 me(global.wm.nextPos.x,global.wm.nextPos.y);
	Dist dist = l.getDistanceWithPoint(me);

	int steps;
	steps = int(global.wm.ballVel.mod() * 7 + 0.5 + EPS);
	if(global.wm.ballVel.mod()<=0.06){
		Angle diff = fabs(Normalize(b.vel.ang()-(global.wm.nextPos-b.pos).ang()));
		if(diff<10)
	       ball_s=b;
		else
			ball_s = predictor.predictBallStatusAfterNrCycle(b,steps);
		desired_ang = (target-ball_s.pos).ang();
		p=Vector3(1,desired_ang+180,0,POLAR);
		p.setMagnitude(r);
		dest = p+ball_s.pos;

#ifdef WIN32
	ShowCircle(dest.x,dest.y,0.22,"0 255 0 2");
#endif

		return runTo(dest,desired_ang);
	}

	desired_ang = (target-ST->myITInfo.IT_point).ang();
	int cyc = predictor.predictCycToReachPoint(Vector3(ST->myITInfo.IT_point.x,ST->myITInfo.IT_point.y),a,desired_ang);

	//==consider the ball in the air====
	for( ;cyc<100;cyc++) {
		if(predictor.predictBallStatusAfterNrCycle(b,cyc).pos.z<=0.5) {
			break;
		}
	}

	//====================
	if (cyc==ST->myITInfo.nCyc) {
		ball_s = predictor.predictBallStatusAfterNrCycle(b,ST->myITInfo.nCyc);
		desired_ang = (target-ball_s.pos).ang();
		p=Vector3(1,desired_ang+180,0,POLAR);
		p.setMagnitude(r);
		dest = p+ball_s.pos;
	} else {
		if(cyc>ST->myITInfo.nCyc){
			for(i=1;i<100;i++){
				ball_s = predictor.predictBallStatusAfterNrCycle(b,i+ST->myITInfo.nCyc);
				desired_ang = (target-ball_s.pos).ang();
				p=Vector3(1,desired_ang+180,0,POLAR);
				p.setMagnitude(r);
				dest = p+ball_s.pos;
				cyc = predictor.predictCycToReachPoint(dest,a,desired_ang);
	
				if(cyc<=(i+ST->myITInfo.nCyc)){
					break;
				}
			}
		} else {

			for(i=ST->myITInfo.nCyc-1;i>0;i--){
				ball_s = predictor.predictBallStatusAfterNrCycle(b,i);
				desired_ang = (target-ball_s.pos).ang();
				p=Vector3(1,desired_ang+180,0,POLAR);
				p.setMagnitude(r);
				dest = p+ball_s.pos;

				cyc = predictor.predictCycToReachPoint(dest,a,desired_ang);
				if(cyc>i){
					ball_s = predictor.predictBallStatusAfterNrCycle(b,i+1);
					desired_ang = (target-ball_s.pos).ang();
					p=Vector3(1,desired_ang+180,0,POLAR);
					p.setMagnitude(r);
					dest = p+ball_s.pos;
					break;
				}
			}
		}
	}

#ifdef WIN32
	ShowCircle(dest.x,dest.y,0.22,"0 255 0 2");
#endif

	return runTo(dest,desired_ang);
}

int Intercept::predictITCyc(Vector3 target,AgentStatus a,AgentStatus ball)
{
	int cyc=0;
	return cyc;
}

/*
	Intercept point
*/
void Intercept::predictITPoint(Vector3 target,
							   AgentStatus a, AgentStatus ball,
							   Vector3 &IT_ballPos, Vector3 &IT_destPos)
{
}

void Intercept::Init(Situation *st,ServerSettings *ss)
{
	ST = st;
	SS = ss;
}

SoccerCommand Intercept::runTo(Vector3 dest, Angle ang_to_kick)
{
	Vector3 F;
	AgentStatus a;
	a.vel = global.wm.nextVel;
	a.pos = global.wm.nextPos;

	F = predictor.predictForceToReachPoint(dest,a,ang_to_kick);
	return SoccerCommand(CT_DRIVE,F);
}

SoccerCommand Intercept::runTo(Vector3 dest)
{
	Vector3 F;
	AgentStatus a;
	a.vel = global.wm.nextVel;
	a.pos = global.wm.nextPos;

	F = predictor.predictForceToReachPoint_forStopBall(dest,a);
	return SoccerCommand(CT_DRIVE,F);
}

SoccerCommand Intercept::interceptForDestroy(Vector3 target, AgentStatus a, AgentStatus b)
{
	return SoccerCommand();
}

SoccerCommand Intercept::interceptForStopBall(AgentStatus a, AgentStatus b)
{
	int i;
	AgentStatus ball_s;

	Line l;
	Angle b_angle(b.vel.ang());
	l=l.makeLineFromPositionAndAngle(Vector3(global.wm.ballPos.x,global.wm.ballPos.y),global.wm.ballVel.ang());
	Vector3 me(global.wm.nextPos.x,global.wm.nextPos.y);
	Dist dist = l.getDistanceWithPoint(me);
	Vector3 adjust(0,0,0);
	if(fabs(b_angle)>=90&&global.wm.ballVel.mod()>0.15) {
		adjust=Vector3(0.33*cosDeg((b_angle+b_angle*180/fabs(b_angle))/2),0.33*sinDeg((b_angle+b_angle*180/fabs(b_angle))/2),0);
	} else {
		adjust=Vector3(-0.3,0,0);
	}

	if(dist<0.3&&!ST->isBallInAir()) {
		Vector3 dest=l.getPointOnLineClosestTo(me);
		double adjusty=global.wm.myPos.y-dest.y;
		dest=Vector3(b.pos.x,b.pos.y-adjusty,0.22);
#ifdef WIN32
        ShowCircle(dest,0.22,"0 255 0 2");
        ShowLine(b.pos,ball_s.pos,"255 120 0");
#endif
		return runTo(dest);
	}
	
	for(i=1;i<100;i++) {
		ball_s = predictor.predictBallStatusAfterNrCycle(b,i);
		Vector3 dest=ball_s.pos+adjust;
		int cyc = predictor.predictCycToReachPoint_forStopBall(dest,a);
		
		if(ball_s.pos.x<X(-100)&&fabs(ball_s.pos.y)<5) {
			Vector3 dest=l.getPointOnLineClosestTo(me);
			return runTo(dest);
		}

		if(predictor.predictBallStatusAfterNrCycle(b, i).pos.z>=0.5)
			continue;
		
		if(i>=cyc||i==99) {
#ifdef WIN32
		    ShowCircle(dest,0.22,"0 255 0 2");
		    ShowLine(b.pos,ball_s.pos,"255 120 0");
#endif
			return runTo(dest);	
		}
	}

	return runTo(ball_s.pos);
}

SoccerCommand Intercept::interceptForCatchBall(AgentStatus a, AgentStatus b)
{
	int i;
	AgentStatus ball_s;
	Line l;

	Angle b_angle(b.vel.ang());
	l=l.makeLineFromPositionAndAngle(Vector3(global.wm.ballPos.x,global.wm.ballPos.y),global.wm.ballVel.ang());
	Vector3 me(global.wm.nextPos.x,global.wm.nextPos.y);
	Dist dist = l.getDistanceWithPoint(me);
	Vector3 adjust(0,0,0);
	Vector3 dest;

	if(dist<0.9&&!ST->isBallInAir()) {
		dest=l.getPointOnLineClosestTo(me);
		double adjusty=global.wm.myPos.y-dest.y;
		dest=Vector3(b.pos.x,b.pos.y-adjusty,0.22);
#ifdef WIN32
        ShowCircle(dest,0.22,"0 255 0 2");
        ShowLine(b.pos,ball_s.pos,"255 120 0");
#endif
		return runTo(dest);
	}
	
	for(i=1;i<100;i++) {
		int flag=0;
		ball_s = predictor.predictBallStatusAfterNrCycle(b,i);
		if(ST->isBallInAir()&&b.pos.z>0.4) {
			dest.x=-X(100)+0.5;
			dest.y=l.getYGivenX(dest.x);
			return runTo(dest);
		}

		Vector3 agent_ball=a.pos-ball_s.pos;
		Angle adjustang=agent_ball.ang();
		double adjustmod=0.9;
		if((ball_s.pos-me).mod()<0.9) {
			flag=1;
			adjustmod=(ball_s.pos-me).mod()-0.2;
			if(adjustmod<EPS) adjustmod=0;
		}

		adjust=Vector3(adjustmod,adjustang,0,POLAR);
		dest=ball_s.pos+adjust;
		int cyc = predictor.predictCycToReachPoint_forStopBall(dest,a);
		if(ball_s.pos.x<X(-100)&&fabs(ball_s.pos.y)<SS->mGoalWidth) {
			dest=l.getPointOnLineClosestTo(me);
			return runTo(dest);
		}

		if(i>=cyc||i==99||flag==1) {
#ifdef WIN32
            ShowCircle(dest,0.22,"0 255 0 2");
            ShowLine(b.pos,ball_s.pos,"255 120 0");
#endif
			return runTo(dest);	
		}
	}

	return runTo(ball_s.pos);
}