kicknrun.cpp

robocup3d源化码 Nexus3D.tar.gz

/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

this file is part of rcssserver3D
Fri May 9 2003
Copyright (C) 2002,2003 Koblenz University
Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
$Id: kicknrun.cpp,v 1.2 2004/02/12 14:07:21 fruit Exp $

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; version 2 of the License.

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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "kicknrun.h"
#include <strstream>
#include "global.h"


using namespace zeitgeist;
using namespace std;
using namespace boost;
using namespace oxygen;
using namespace salt;

KickNRun::KickNRun() : Soccer()
{
}

KickNRun::~KickNRun()
{
}

void KickNRun::BehaveBeforeKickOff()
{
	MyPlayer[1].move_radius=20;//30;
	MyPlayer[2].move_radius=20;//30;
	MyPlayer[3].move_radius=15;//25;
	MyPlayer[4].move_radius=15;//25;
	MyPlayer[5].move_radius=20;//30;
	MyPlayer[6].move_radius=20;//30;
	MyPlayer[7].move_radius=15;//20;
	MyPlayer[8].move_radius=20;//30;
	MyPlayer[9].move_radius=15;//25;
	MyPlayer[10].move_radius=15;//20;
	MyPlayer[11].move_radius=15;//25;
	self.move_radius = MyPlayer[MyNumber].move_radius;



	MyPlayer[1].home_pos = Vector3f(-47,0,0);
	MyPlayer[2].home_pos = Vector3f(-20,-12,0);
	MyPlayer[3].home_pos = Vector3f(-25,-5,0);
	MyPlayer[4].home_pos = Vector3f(-25,5,0);
	MyPlayer[5].home_pos = Vector3f(-20,12,0);
	MyPlayer[6].home_pos = Vector3f(-10,-16,0);
	MyPlayer[7].home_pos = Vector3f(-15,0,0);
	MyPlayer[8].home_pos = Vector3f(-10,16,0);
	MyPlayer[9].home_pos = Vector3f(0,20,0);
	MyPlayer[10].home_pos = Vector3f(-5,0,0);
	MyPlayer[11].home_pos = Vector3f(0,-20,0);
	self.home_pos = MyPlayer[MyNumber].home_pos;
    switch(MyNumber){
    	case 1:
		Beam(Vector3f(-47,0,0));
		break;
	case 2:
		Beam(Vector3f(-20,-12,0));
		break;
    	case 3:
		Beam(Vector3f(-25,-5,0));
		break;
    	case 4:
		Beam(Vector3f(-25,5,0));
		break;
    	case 5:
		Beam(Vector3f(-20,12,0));
		break;
    	case 6:
		Beam(Vector3f(-10,-12,0));
		break;
    	case 7:
		Beam(Vector3f(-15,0,0));
		break;
    	case 8:
		Beam(Vector3f(-10,12,0));
		break;
    	case 9:
		Beam(Vector3f(-1,20,0));
		break;
    	case 10:
		Beam(Vector3f(-5,0,0));
		break;
    	case 11:
		Beam(Vector3f(-1,-20,0));
		break;
    	default:
		Beam(Vector3f(-0.1,0,0));
		break;
    }
}

void KickNRun::BehaveKickOff()
{
    TTeamIndex ti = mWM->GetMyTeam();
    if (ti == TI_NONE)
        {
            return;
        }

    TPlayMode myKickOff = (mWM->GetMyTeam() == TI_LEFT) ?
        PM_KickOff_Left : PM_KickOff_Right;

    if (mWM->GetPlayMode() == myKickOff)
        {
            BehaveMyKickOff();
            GetLog()->Debug() << "MyKickOff\n";
        } else
            {
                BehaveTheirKickOff();
                GetLog()->Debug() << "TheirKickOff\n";
            }
}

void KickNRun::BehaveMyKickOff()
{
    // for now, just kick and run
    //BehaveBeforeKickOff();
    if(do_beam_on_kickof){
	switch(MyNumber){
		case 1:
			Beam(Vector3f(-47,0,0));
			break;
		case 2:
			Beam(Vector3f(-20,-12,0));
			break;
		case 3:
			Beam(Vector3f(-25,-5,0));
			break;
		case 4:
			Beam(Vector3f(-25,5,0));
			break;
		case 5:
			Beam(Vector3f(-20,12,0));
			break;
		case 6:
			Beam(Vector3f(-10,-12,0));
			break;
		case 7:
			Beam(Vector3f(-15,0,0));
			break;
		case 8:
			Beam(Vector3f(-10,12,0));
			break;
		case 9:
			Beam(Vector3f(-1,20,0));
			break;
		case 10:
			Beam(Vector3f(-5,0,0));
			break;
		case 11:
			Beam(Vector3f(-1,-20,0));
			break;
		default:
			Beam(Vector3f(-0.1,0,0));
			break;
        }
	do_beam_on_kickof=false;
    }
    else{
    	BehavePlayOn();
    }
}

void KickNRun::BehaveTheirKickOff()
{
    // do nothing
    //BehaveBeforeKickOff();
    //Drive(Vector3f(0,0,0));
    if(do_beam_on_kickof){
        switch(MyNumber){
		case 1:
			Beam(Vector3f(-47,0,0));
			break;
		case 2:
			Beam(Vector3f(-20,-12,0));
			break;
		case 3:
			Beam(Vector3f(-25,-5,0));
			break;
		case 4:
			Beam(Vector3f(-25,5,0));
			break;
		case 5:
			Beam(Vector3f(-20,12,0));
			break;
		case 6:
			Beam(Vector3f(-10,-12,0));
			break;
		case 7:
			Beam(Vector3f(-15,0,0));
			break;
		case 8:
			Beam(Vector3f(-10,12,0));
			break;
		case 9:
			Beam(Vector3f(0,20,0));
			break;
		case 10:
			Beam(Vector3f(-5,0,0));
			break;
		case 11:
			Beam(Vector3f(0,-20,0));
			break;
		default:
			Beam(Vector3f(0,0,0));
			break;
        }
	do_beam_on_kickof=false;
    }
    else   BehavePlayOn();BehavePlayOn();
}

void KickNRun::BehaveKickIn()
{
    TTeamIndex ti = mWM->GetMyTeam();
    if (ti == TI_NONE)
        {
            return;
        }

    TPlayMode myKickIn = (mWM->GetMyTeam() == TI_LEFT) ?
        PM_KickIn_Left : PM_KickIn_Right;

    if (mWM->GetPlayMode() == myKickIn)
        {
            BehaveMyKickIn();
            GetLog()->Debug() << "MyKickIn\n";
        } else
            {
                BehaveTheirKickIn();
                GetLog()->Debug() << "TheirKickIn\n";
            }
}

void KickNRun::BehaveMyKickIn()
{
    // for now just kick and run
    BehavePlayOn();
    /*
    if(self.dist2ball > MyPlayer[FieldInfo.NearestTeammate2(ball.pos)].dist2ball)
    {
	if(self.IsGoalie)
		goalie.SmartGoalie();
	else
    		positioning.Position();
    }
    else
    {
    	int num = FieldInfo.NearestTeammate2(ball.pos);
	float angle = NormalizeAngle(Rad2Deg(atan2((MyPlayer[num].pos[1]-ball.pos[1]),(MyPlayer[num].pos[0]-ball.pos[0]))),-180.0);
	if(AdjustKickPos.AdjustNeeded(angle, 10))
		AdjustKickPos.MoveAroundBall(angle);
	else
	{
		action_info.kick_angle = 30;
		action_info.kick_direction = angle;
		action_info.kick_power = 100;
		action_info.do_kick = true;
	}
    }
    PostAction();
    */
}

void KickNRun::BehaveTheirKickIn()
{
    // do nothing
    //Drive(Vector3f(0,0,0));
    BehavePlayOn();
    /*
	if(self.IsGoalie)
		goalie.SmartGoalie();
	else
		positioning.Position();
	PostAction();
    */
}

void KickNRun::BehavePlayOn()
{


	PredictBallPos();
	//estimation of ball pos
	DoLog(LOG_GOALIE,"ball.pos=(%f,%f,%f)  kickdist=%f  ball.dist=%f",ball.pos[0],ball.pos[1],ball.pos[2],KickDistance,ball.distance);
	DoLog(LOG_GOALIE,"ball.vel=(%f,%f,%f)",ball.vel[0],ball.vel[1],ball.vel[2]);
	DoLog(LOG_GOALIE,"FieldLength=%f FieldWidth=%f FieldHeight;=%f GoalWidth=%f GoalDepth=%f GoalHeight=%f BorderSize=%f",FieldLength,FieldWidth,FieldHeight,GoalWidth,GoalDepth,GoalHeight,BorderSize);
	DoLog(LOG_GOALIE,"Self.pos=(%f,%f,%f) ",self.pos[0],self.pos[1],self.pos[2]);

    	TTeamIndex ti = mWM->GetMyTeam();

	if(self.IsGoalie){
		goalie.SmartGoalie();
	}
	else
	{
	//if (ti == TI_RIGHT) return;
		/*int player_num=0;
		float min_dist2ball=100;
		for(int i=1;i<=11;i++) if(MyPlayer[i].dist2ball<min_dist2ball&&i!=MyNumber) 	min_dist2ball=MyPlayer[i].dist2ball;
		if(ball.distance>min_dist2ball){
		}
		else{
			if(ball.distance>4){
				action_info.x = ball.pos[0] ;
				action_info.y = ball.pos[1] ;
				action_info.do_drive = true ;
			}
			else if(!AdjustKickPos.AdjustNeeded(-179)){
				DoLog(LOG_GOALIE,"Kick");
				Kick(20,100);
			}
			else {
				Vector3f Pos=AdjustKickPos.MoveAroundBall(-179);
				DoLog(LOG_GOALIE,"adjust nedded    pos(%f,%f)",Pos[0],Pos[1]);
				action_info.x = Pos[0] ;
				action_info.y = Pos[1] ;
				action_info.do_drive = true ;

			}
		}*/
	/*	if(MyNumber==11&&self.pos[0]<10){
			 GoTo(40,10,0);
			action_info.x = 40 ;
			action_info.y = 10 ;
			action_info.do_drive = true ;
		}
			DoLog(LOG_GOALIE,"vel(%f,%f,%f)  pos(%f,%f,%f)",self.vel[0],self.vel[1],self.vel[2],self.pos[0],self.pos[1],self.pos[2]);

		//DoLog(LOG_GOALIE,"this agent is not goalie");*/
		/*defence*/
		float length = 1000.0;
		int index;
		for(int i=1 ; i<=11 ; i++)
			if((TheirPlayer[i].pos - Vector3f(-FieldLength/2,0,0)).Length() < length){
				length = (TheirPlayer[i].pos - Vector3f(-FieldLength/2,0,0)).Length();
				index = i;
			}
		int 	NearestTeammate2ball=2;
		
		if((fabs(ball.vel[0])>0.75||fabs(ball.vel[1])>0.75)&&ball.distance>0.75){
			for(int i=2;i<=8;i++){
				if((MyPlayer[i].pos-predicted_ball_pos[stop_index]).Length()<(MyPlayer[NearestTeammate2ball].pos-predicted_ball_pos[stop_index]).Length()) NearestTeammate2ball=i;
			}
			if(MyNumber<9&&(predicted_ball_pos[stop_index]-Vector3f(-FieldLength/2,0,0)).Length()<30&&(TheirPlayer[index].pos-predicted_ball_pos[stop_index]).Length()<3&&NearestTeammate2ball==MyNumber){
				float m=(predicted_ball_pos[stop_index][1]-self.pos[1])/(predicted_ball_pos[stop_index][0]-self.pos[0]);
				float y=m*(-FieldLength/2-predicted_ball_pos[stop_index][0])+predicted_ball_pos[stop_index][1];
				if ((self.pos-predicted_ball_pos[stop_index]).Length() < KickDistance){
						//Kick(20,100);
						if(y>-3.80&&y<3.80){
							action_info.kick_angle=35;
							action_info.kick_direction=0;
							action_info.kick_power=100;
							action_info.do_kick=true;
							DoLog(LOG_GOALIE,"clear1");
						}
						else{
							action_info.kick_angle=25;
							action_info.kick_direction=0;
							action_info.kick_power=100;
							action_info.do_kick=true;
							DoLog(LOG_GOALIE,"clear2");
						}
				}
				else{
						action_info.x = predicted_ball_pos[stop_index][0]-KickDistance ;
						action_info.y = predicted_ball_pos[stop_index][1] ;
						action_info.do_drive = true ;
						DoLog(LOG_GOALIE,"clear3");
				}
				
			}
			else{
				if(!handleball.TryHandle())
				{
					if(!intercept.TryIntercept())
					{
						/*goalie2*/if(!positioning.Position()) goalie.SmartGoalie();
					}
				}	
			}
		}
		else{
			for(int j=2;j<=8;j++){
				if(MyPlayer[j].dist2ball<MyPlayer[NearestTeammate2ball].dist2ball) NearestTeammate2ball=j;
			}
			if(MyNumber<9&&(ball.pos-Vector3f(-FieldLength/2,0,0)).Length()<30&&TheirPlayer[index].dist2ball<3&&NearestTeammate2ball==MyNumber){
				float m=(ball.pos[1]-self.pos[1])/(ball.pos[0]-self.pos[0]);
				float y=m*(-FieldLength/2-ball.pos[0])+ball.pos[1];
				if (ball.distance < KickDistance){
						//Kick(20,100);
						if(y>-3.80&&y<3.80){
							action_info.kick_angle=35;
							action_info.kick_direction=0;
							action_info.kick_power=100;
							action_info.do_kick=true;
							DoLog(LOG_GOALIE,"clear1");
						}
						else{
							action_info.kick_angle=25;
							action_info.kick_direction=0;
							action_info.kick_power=100;
							action_info.do_kick=true;
							DoLog(LOG_GOALIE,"clear2");
						}
				}
				else{
						action_info.x = ball.pos[0]-KickDistance ;
						action_info.y = ball.pos[1] ;
						action_info.do_drive = true ;
						DoLog(LOG_GOALIE,"clear3");
				}
				
			}
			else{
				if(!handleball.TryHandle())
				{
					if(!intercept.TryIntercept())
					{
						/*goalie2*/if(!positioning.Position()) goalie.SmartGoalie();
					}
				}	
			}
		}	

	}
//////////////
  //		action_info.kick_angle=20;
//		action_info.kick_direction=0;