dribble_around.c

Brainstormers(头脑风暴)队是05年robocup冠军,这是05年Brainstormers公布的源代码,Brainstormers是robocup老牌的德国强队

/*
Brainstormers 2D (Soccer Simulation League 2D)
PUBLIC SOURCE CODE RELEASE 2005
Copyright (C) 1998-2005 Neuroinformatics Group,
                        University of Osnabrueck, Germany

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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

// vim:ts=2:sw=2:ai:fdm=marker:fml=3
// includes {{{1
#include "log_macros.h"
#include "../policy/positioning.h"
#include "ws_info.h"
#include "options.h"
#include "mdp_info.h"
#include "ws_memory.h"

#include "mystate.h"
#include "intention.h"

#include "dribble_around.h"

// Log macros {{{1
#if 1   /* 1: debugging; 0: no debugging */
#define POL(XXX)   LOG_POL(0,<<"DribbleAround: "<<XXX)
#define POL2(XXX)  LOG_POL(1,<<"DribbleAround: "<<XXX)
#define DRAW(XXX)  LOG_POL(0,<<_2D<<XXX)
#define DRAW2(XXX) LOG_POL(1,<<_2D<<XXX)
#else
#define POL(XXX) 
#define POL2(XXX) 
#define DRAW(XXX) 
#define DRAW2(XXX) 
#endif
// Drawing macros {{{1
#define MARK_POS(P,C) DRAW(C2D((P).x,(P).y,0.3,#C));
#define MARK_BALL(B,C) DRAW(C2D(B.pos.x,B.pos.y,0.3,#C));
#define MARK_STATE(P,C) DRAW(C2D(P.pos.x,P.pos.y,P.kick_radius,#C));\
  __ang.init_polar(P.kick_radius,P.ang.get_value());\
  __ang+=P.pos;\
	DRAW(L2D(P.pos.x,P.pos.y,__ang.x,__ang.y,#C));
#define MARK_PPLAYER(P,C) DRAW(C2D(P->pos.x,P->pos.y,P->kick_radius,#C));
// Standard macros {{{1
#define SGN(X) ((X<0)?-1:1)
#define SQR(X) (X * X)
#define QUBE(X) (X * X * X)
// skill constants {{{1
#define MAX_ANGLE_TO_DEST DEG2RAD(10)



// Constructing an object {{{1
DribbleAround::DribbleAround(){
	basic_cmd = new BasicCmd;
	dribble_straight = new DribbleStraight;
	go2pos = new NeuroGo2Pos;
	searchball = new SearchBall;
	onestepkick = new OneStepKick;
	intercept = new InterceptBall;
	holdturn = new OneTwoHoldTurn;
	dribbleTo = HIS_GOAL_CENTER;
	request = DAREQ_NONE;
	requestTime = 0;
	didDribble = false;
	neckReqSet=false;
	dribbleInsecure=false;
};
//DribbleAround::DribbleAround(const DribbleAround&){};
//DribbleAround DribbleAround::operator=(const DribbleAround&){};
DribbleAround* DribbleAround::myInstance = NULL;

DribbleAround* DribbleAround::getInstance() {
	if(myInstance==NULL){
		myInstance = new DribbleAround();
	}
	return myInstance;
}

// set a request
void DribbleAround::setRequest(DARequest dar){
	request = dar;
	requestTime = WSinfo::ws->time;
}

// get_dribble_straight_cmd() {{{1
bool DribbleAround::get_dribble_straight_cmd(Cmd& cmd){
	POL("Let dribble_straight handle situation, I cannot");
	return dribble_straight->get_cmd(cmd);
}

// getRelevantOpponent() {{{1
PPlayer DribbleAround::getRelevantOpponent(){ 
	static const float farAway     = 10;
	static const float closeBehind = 4;
	
  WSpset opps = WSinfo::valid_opponents;

	Vector toFarAway = dribbleTo - WSinfo::me->pos;
	toFarAway.normalize(farAway);
	Vector justBehind = dribbleTo-WSinfo::me->pos;
	justBehind.normalize(closeBehind);
	opps.keep_and_sort_closest_players_to_point(6,WSinfo::me->pos);
	opps.keep_players_in_quadrangle(WSinfo::me->pos-justBehind,WSinfo::me->pos+toFarAway,8,20);

	// no opp
	if(opps.num==0) return NULL;
	
	PlayerState p;
	p.setAssumeToPos(opps[0],WSinfo::me->pos);

	// one opp, reaches my position
	if(p.pos.distance(WSinfo::me->pos)<WSinfo::me->kick_radius + p.kick_radius)
		return opps[0];

	// check other opps, if they reach me, choose them
	for(int i=1;i<opps.num;i++){
		p.setAssumeToPos(opps[i],WSinfo::me->pos);
		if(p.pos.distance(WSinfo::me->pos)<WSinfo::me->kick_radius + p.kick_radius)
			return opps[i];
	}

	// return closest.
	return opps[0];
}

float pow(int a, float y, float z){
	if(a<=0) return 1;
	return (a==1)?y:pow(a-1,y*z,z);
}

// getCmdKickToDest() {{{1
bool DribbleAround::getCmdKickToDest(Cmd& cmd, const Vector& dest, bool keepInKickRadius, bool force){
	POL2("In getCmdKickToDest()");
	//float distToDest = (nextMeNA.pos - dest).norm();
	cmd.cmd_main.unset_lock();
	
	if(keepBallSafe) 
		keepInKickRadius=true;
	
	// needed for onestepkick, definitely the angle to BALL, NOT me.
	ANGLE toDest = (dest - WSinfo::ball->pos).ARG();
	static const float initSpeedStart = 0.3;
	float initSpeed = initSpeedStart;
	float bestInitSpeedToDest = 0;
	static const float speedIncr = 0.1;
	float distDestToNewBall;
	float bestDistDestToNewBall = 1E6;
	float distOfBestToKickRadius = 1E6;
	float minDistToBall = 1.25*(ServerOptions::player_size + ServerOptions::ball_size);
	float kickMax = 2.0; // TODO: !? was 1.4
	bool thruOpp = false;
	bool inOpp = false;
	Cmd tmpCmd;
	Vector new_my_pos,new_my_vel,new_ball_pos,new_ball_vel;
	ANGLE new_my_ang;

	if(    fabs(dest.x)>FIELD_BORDER_X-.2 
			|| fabs(dest.y)>FIELD_BORDER_Y-.2){
		POL2("getCmdKickToDest: dest too close to field border");
		if(!force)
		  return false;
	}

	// can't keep ball at a position -- bug in onestepkick!?
	bool tooShortKick = dest.distance(WSinfo::ball->pos)<0.1;
	if(tooShortKick) {
		POL2("Too close!?");
		// if(!force) return false;
		toDest = WSinfo::ball->vel.ARG() - ANGLE(PI);
	}
	
	//bool keepInKickRadius = (nextMeNA.pos.distance(dest)<WSinfo::me->kick_radius);
	POL2("Calculating target kick speed, keepInKickRadius = "<< keepInKickRadius);

	bool insecure;
	bool bestIsInsecure;
	Vector lastNewBallPos(nextBall.pos);
	bool distDecreasing=true;
	for(;initSpeed<=kickMax;initSpeed += speedIncr){
		insecure=false;
		tmpCmd.cmd_main.unset_lock();
		onestepkick->kick_in_dir_with_initial_vel(initSpeed,toDest);
		onestepkick->get_cmd(tmpCmd);
		Tools::model_cmd_main(WSinfo::me->pos,WSinfo::me->vel,WSinfo::me->ang,WSinfo::ball->pos,WSinfo::ball->vel,tmpCmd.cmd_main,new_my_pos,new_my_vel,new_my_ang,new_ball_pos,new_ball_vel,false);
		DRAW2(C2D(new_ball_pos.x,new_ball_pos.y,0.3,"green"));

		distDecreasing=
			initSpeed==initSpeedStart
			||lastNewBallPos.sqr_distance(dest)>new_ball_pos.sqr_distance(dest);
		lastNewBallPos = new_ball_pos;
		
		if(dest.sqr_distance(new_ball_pos)>SQR(.4)
				&& !distDecreasing
				&&  initSpeed>0.7
				&& thruOpp){
			POL2("Have to kick too far ahead to avoid opponent, stopping at initSpeed = "<<initSpeed);
			if(!force)
				break;
		}
		
		if(opp && new_ball_pos.distance(nextOppToMe.pos)<nextOppToMe.kick_radius){
			POL2("nextOppToMe would get ball, ruling out initSpeed = "<<initSpeed);
			thruOpp = true; // ball goes thru kick_radius of opp
			inOpp   = true;
			if(!force) continue;  
			insecure=true;
		}
		if(opp && 0.8<Tools::get_tackle_success_probability(nextOppToMe.pos,new_ball_pos,nextOppToMe.ang.get_value())){
			POL2("nextOppToMe would be able to tackle, ruling out initSpeed = "<<initSpeed);
			if(!force) continue;
			insecure=true;
		}
		if(opp && new_ball_pos.distance(opp->pos)<opp->kick_radius){
			POL2("opp would get ball, ruling out initSpeed = "<<initSpeed);
			thruOpp = true; // ball goes thru kick_radius of opp
			inOpp   = true;
			if(!force) continue;  
			insecure=true;
		}
		if(opp && new_ball_pos.distance(nextOppNA.pos)<nextOppNA.kick_radius){
			POL2("opponentNA would get ball, ruling out initSpeed = "<<initSpeed);
			if(!force) continue;  
			insecure=true;
		}
		// TODO: better: p!?
		float hisTackleProb = (opp?Tools::get_tackle_success_probability(nextOppToMe.pos,new_ball_pos,nextOppToMe.ang.get_value()):0);
		if(hisTackleProb>0.9){
			POL2("Player to dest could tackle, ruling out initSpeed = "<<initSpeed);
			if(!force) continue;  
			insecure=true;
		}
		//POL2("opponent could not get ball, allowing initSpeed = "<<initSpeed);
		inOpp   = false;

		distDestToNewBall = dest.distance(new_ball_pos);

		// more efficient, shouldnt get any better from here on
		if(distDestToNewBall>bestDistDestToNewBall && keepInKickRadius) break;

		if(distDestToNewBall<bestDistDestToNewBall                                // closer to dest
				&& (force || nextMeNA.pos.distance(dest)>minDistToBall)               // no collision
				&&(!keepInKickRadius                                                  // dont care for kick_radius
					||(new_ball_pos.distance(nextMeNA.pos)<WSinfo::me->kick_radius))){  // or in kick_radius
			//POL2("New bestInitSpeedToDest="<<initSpeed);
			bestInitSpeedToDest = initSpeed;
			bestIsInsecure = insecure;
			bestDistDestToNewBall = distDestToNewBall;
			distOfBestToKickRadius = WSinfo::me->kick_radius-new_ball_pos.distance(nextMeNA.pos);
		}
	}

	if(inOpp || bestInitSpeedToDest == 0){
		POL2("No valid kick found.");
		return false;
	}
	
	/* TODO: Was this necessary?
	if(keepInKickRadius && distOfBestToKickRadius<0.20){
		POL2("too close to kickable margin, reducing initSpeed");
		bestInitSpeedToDest *= 0.9;
	}else if(thruOpp){
		POL2("kick thru opp, kick a little more");
		bestInitSpeedToDest += .5*speedIncr;
	}
	*/


	// Now check for insecurities
	PlayerState p;
	if(bestIsInsecure)  // do not set to false, might be true already 
		dribbleInsecure = true;
	if(tooShortKick)
		dribbleInsecure = true;
	if(opp){
		p.setAssumeToPos(opp,dest);
		bool oppStandsInMe = false && nextMeNA.pos.distance(p.pos)<2.5*ServerOptions::player_size;
		if(!oppStandsInMe && p.pos.distance(dest)<p.kick_radius){
			POL2("getCmdKickToDest: dest can be reached by opponent!");
		}
	}
	/*
	if(Tools::get_tackle_success_probability(p.pos,dest,p.ang.get_value())>.90)
		dribbleInsecure = true;
		*/

	POL2("Found position is risky: "<<dribbleInsecure);
	
	onestepkick->kick_in_dir_with_initial_vel(bestInitSpeedToDest,toDest);
	if(!onestepkick->get_cmd(cmd)){
		// TODO: Why does this return false most of the time?
		POL("OneStepKick returned false: STRANGE");
		//dribbleInsecure = true;
		//return false;
	} 
	return true;
}


// getNextAction(opp) {{{1
DribbleAround::Action DribbleAround::getNextAction(PPlayer& opp){
	ANGLE toGoal = Tools::my_angle_to(dribbleTo);
	bool bodyAngleOK = fabs(toGoal.get_value_mPI_pPI())<MAX_ANGLE_TO_DEST;
	bool bodyAngleOffALot = !bodyAngleOK 
		&& fabs(toGoal.get_value_mPI_pPI())>DEG2RAD(100);

	static const float maxDistToBall = 0.80*WSinfo::me->kick_radius;

	bool isOppGoalie = opp->number == WSinfo::ws->his_goalie_number;

	// Are there teammates also in possession of ball?
	WSpset team = WSinfo::valid_teammates_without_me;
	team.keep_and_sort_closest_players_to_point(1,WSinfo::ball->pos);
	bool dontKick = 
		   team.num>0
		&& (team[0]->pos.distance(WSinfo::ball->pos)<team[0]->kick_radius)
		&& (team[0]->number > WSinfo::me->number);
	if(dontKick)
		POL("getNextAction: I have a lower number than teammate who also controls ball -- not kicking");

	// Two just ahead?
	WSpset opps = WSinfo::valid_opponents;
	opps.keep_and_sort_closest_players_to_point(2,WSinfo::me->pos);

	bool twoAgainstMe = opps.num>1
		&& fabs(Tools::my_angle_to(opps[0]->pos).get_value_mPI_pPI())<DEG2RAD(45)
		&& fabs(Tools::my_angle_to(opps[1]->pos).get_value_mPI_pPI())<DEG2RAD(45)
		&& WSinfo::me->pos.distance(opps[0]->pos)<2
		&& WSinfo::me->pos.distance(opps[1]->pos)<2;

	float hisTackleProb = (opp?Tools::get_tackle_success_probability(opp->pos,WSinfo::ball->pos,opp->ang.get_value()):0);
	float myTackleProb = Tools::get_tackle_success_probability(WSinfo::me->pos,WSinfo::ball->pos,WSinfo::me->ang.get_value());

	if(hisTackleProb>.85){
		POL("getNextAction: Opponent can tackle -- dangerous!");
		dribbleInsecure = true;
	}
	/*
	if(hisTackleProb>.95 && myTackleProb>.8){
		POL("getNextAction: Tackle needed: Opp can tackle, I can tackle!");
		return DA_TACKLE;
	}*/
	/*
	if(twoAgainstMe
			&& myTackleProb>.8){
		POL("getNextAction: Tackle needed: two against me");
		return DA_TACKLE;
	}*/
	if(nextOppToBall.pos.distance(nextBall.pos)<nextOppToBall.kick_radius+ServerOptions::ball_size){   // opponent gets ball
		POL("getNextAction: Kick needed: Opp can reach ball (1)");
		if(!dontKick)
			return DA_KICK;
	}
	if(opp->pos.distance(nextBall.pos)<opp->kick_radius+ServerOptions::ball_size){   // opponent gets ball
		POL("getNextAction: Kick needed: Opp can reach ball (2)");
		if(!dontKick)
			return DA_KICK;
	}
	/*
	 * TODO: Why do I need this?
	if((nextOppToBall.pos-nextMeNA.pos).norm()<(WSinfo::me->kick_radius+nextOppToMe.kick_radius-0.5)){
		POL("getNextAction: Kick needed: Overlapping kick radii");
		if(!dontKick)
			return DA_KICK;
	}
	*/
	if(Tools::get_tackle_success_probability(nextOppToBall.pos,nextBall.pos,nextOppToMe.ang.get_value())>.70){
		POL("getNextAction: Kick needed: Otherwise opp could tackle next cycle");
		if(!dontKick)
			return DA_KICK;
	}

	Vector inMyDir;
	inMyDir.init_polar(0.1,WSinfo::me->ang);
	switch(request){
		case DAREQ_NONE:
			break;
		case DAREQ_KICK:
			POL("getNextAction: Kick needed: Was requested");
			if(!dontKick) return DA_KICK;
			break;
		case DAREQ_DASH:
			if(WSinfo::me->pos.distance(nextBall.pos) > (WSinfo::me->pos+inMyDir).distance(nextBall.pos)){
				POL("getNextAction: Dash needed: Was requested");
				return DA_DASH;
			}else{
				POL("getNextAction: Ignoring requested dash!");
			}
			break;
		case DAREQ_TURN:
			if(!bodyAngleOK){
				bool looseBall = nextBall.pos.distance(nextMeNA.pos)>0.9*WSinfo::me->kick_radius;
				bool looseBallBehind = looseBall 
					&& fabs((dribbleTo-WSinfo::me->pos).ANGLE_to(nextBall.pos-WSinfo::me->pos).get_value_mPI_pPI())>DEG2RAD(100);
				if(!looseBallBehind){
					POL("getNextAction: Turn needed: Was requested");
					return DA_TURN;
				}else{
					POL("getNextAction: Ignoring requested turn!");
				}
			}
	}
	bool ballWillLeaveField= 
	  	fabs(nextBall.pos.x)>FIELD_BORDER_X-0.5
		||fabs(nextBall.pos.y)>FIELD_BORDER_Y-0.5;
	if(nextMeNA.pos.distance(nextBall.pos)<maxDistToBall)        
	  if(!bodyAngleOK && !ballWillLeaveField){
	    POL("getNextAction: Turn needed");
	    return DA_TURN;