dribble_around.c

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

	old_new_my_ang = new_my_ang;

	bool goingInXDir = dribbleTo.x > WSinfo::me->pos.x + 5;

	const int maxDashes = (!goingInXDir || opp&&opp->age>1) ? 1:8;

	// simulate turns until dir is ok
	int numTurns = 0;
	Value moment;
	while(fabs((aToDribbleTo - new_my_ang).get_value_mPI_pPI())>MAX_ANGLE_TO_DEST){
		if(numTurns>5) break;
		numTurns++;
		tmpCmd.cmd_main.unset_lock();

		// calculate moment. More or less copy 'n' paste from do_turn_inertia (basic_cmd_bms.c)
		moment = (aToDribbleTo-new_my_ang).get_value_mPI_pPI();
		moment = moment * (1.+(WSinfo::me->inertia_moment * new_my_vel.norm()));
		moment = moment >  3.14 ?  3.14 : moment;
		moment = moment < -3.14 ? -3.14 : moment;

		basic_cmd->set_turn(moment);
		basic_cmd->get_cmd(tmpCmd);
		Tools::model_cmd_main(old_new_my_pos,old_new_my_vel,old_new_my_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);
		old_new_my_pos = new_my_pos;
		old_new_my_vel = new_my_vel;
		old_new_my_ang = new_my_ang;
		POL2("getKickForTurn: Angle to dest: "<< (aToDribbleTo-new_my_ang));
	}
	POL2("getKickForTurn: Will need " << numTurns << " cycles to turn around");
	POL2("getKickForTurn: Opponent is left="<<isOppLeft);

	Vector myPosAfterTurn = new_my_pos;
	ANGLE  myAngAfterTurn = new_my_ang;
	
	Vector onMe;
	bool keepBallLeft = ( opp && !isOppLeft)
		                ||(!opp &&  isBallOnLeftSide);
	onMe.init_polar(0.5*WSinfo::me->kick_radius, myAngAfterTurn + ANGLE(DEG2RAD(keepBallLeft?90:-90)));

	bool haventLookedThere = 
		WSmemory::last_seen_in_dir(Tools::my_angle_to(dribbleTo))>3;

	int numDashes   = 0;
	tmpCmd.cmd_main.unset_lock();
	basic_cmd->set_dash(100);
	basic_cmd->get_cmd(tmpCmd);
	WSpset opps;
	Vector catchPoint;
	if(!haventLookedThere) // do not consider dashing if having to turn a lot
		do{
			Tools::model_cmd_main(old_new_my_pos,old_new_my_vel,old_new_my_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);
			opps = WSinfo::valid_opponents;
			catchPoint = new_my_pos+onMe;
			opps.keep_and_sort_closest_players_to_point(1,catchPoint);
			if(opps.num>0)
				if(opps[0]->pos.distance(catchPoint) < opps[0]->kick_radius + (numTurns+numDashes+1)*opps[0]->speed_max*1)
					break;
			if(fabs(catchPoint.x)>FIELD_BORDER_X-1 || fabs(catchPoint.y)>FIELD_BORDER_Y-1)
				break;
			numDashes++;
			old_new_my_pos = new_my_pos;
			old_new_my_vel = new_my_vel;
			old_new_my_ang = new_my_ang;
		}while(numDashes<10);

	POL2("getKickForTurn: Additional "<<numDashes<<" dashes are possible");

	Vector myPosAfterDashes = (numDashes==0) ? myPosAfterTurn : old_new_my_pos;
	ANGLE  myAngAfterDashes = (numDashes==0) ? myAngAfterTurn : old_new_my_ang;

	catchPoint = onMe+myPosAfterDashes;

	bool tooFar;
	Vector dest = getKickDestForBallPosInNCycles(catchPoint, 1+numTurns+numDashes, tooFar);

	bool closeToXBorder = (WSinfo::me->pos.x) > FIELD_BORDER_X-WSinfo::me->kick_radius;
	bool closeToYBorder = (WSinfo::me->pos.y) > FIELD_BORDER_Y-WSinfo::me->kick_radius;
	bool tooCloseToBorder = closeToXBorder || closeToYBorder;

	// OK, now try to get the ball there.

	if(getCmdKickToDest(cmd,dest,false,false)&&!dribbleInsecure){
		POL("getKickForTurn: 1. Kicking ball in \"good\" direction, hope to catch up");
		DRAW(C2D(myPosAfterDashes.x,myPosAfterDashes.y,WSinfo::me->kick_radius,"green"));
		MARK_POS(catchPoint,green);
		setRequest(DAREQ_TURN);
		MARK_POS(dest, blue);
		return true;
	}
	dribbleInsecure=false;

	// it didnt work out, probably I'm in the way. Try some other angles
	// (unchecked! dangerous!)

	onMe.init_polar(0.5*WSinfo::me->kick_radius, myAngAfterTurn + ANGLE(DEG2RAD(keepBallLeft?45:-45)));
	catchPoint = onMe+myPosAfterDashes;
	dest = getKickDestForBallPosInNCycles(catchPoint, 1+numTurns+numDashes, tooFar);
	if(getCmdKickToDest(cmd,dest,false,false) && !dribbleInsecure){
		POL("getKickForTurn: 2. Kicking ball in \"good\" direction, hope to catch up");
		setRequest(DAREQ_TURN);
		DRAW(C2D(myPosAfterDashes.x,myPosAfterDashes.y,WSinfo::me->kick_radius,"green"));
		MARK_POS(catchPoint,green);
		MARK_POS(dest, blue);
		return true;
	}
	dribbleInsecure=false;

	onMe.init_polar(0.5*WSinfo::me->kick_radius, myAngAfterTurn + ANGLE(DEG2RAD(keepBallLeft?135:-135)));
	catchPoint = onMe+myPosAfterDashes;
	dest = getKickDestForBallPosInNCycles(catchPoint, 1+numTurns+numDashes, tooFar);
	if(getCmdKickToDest(cmd,dest,false,false)&&!dribbleInsecure){
		POL("getKickForTurn: 3. Kicking ball in \"good\" direction, hope to catch up");
		DRAW(C2D(myPosAfterDashes.x,myPosAfterDashes.y,WSinfo::me->kick_radius,"green"));
		MARK_POS(catchPoint,green);
		setRequest(DAREQ_TURN);
		MARK_POS(dest, blue);
		return true;
	}
	dribbleInsecure=false;

	if(tooCloseToBorder){
		POL("getKickForTurn: Too close to Border, turn-kick not safe!");
		return false;
	}
	
	if(nextOppToMe.reachesPos){
		Vector safestPos, bestPos;
		getTargetsInMe(safestPos, bestPos);
		if(getCmdKickToDest(cmd,bestPos,true,false)){
			POL("Kicking ball to best position");
			MARK_POS(safestPos, blue);
			return true;
		}
		if(getCmdKickToDest(cmd,safestPos,true,false)){
			POL("Kicking ball to safest position");
			MARK_POS(safestPos, blue);
			return true;
		}
	}

	if(holdturn->is_holdturn_safe()){
		POL("getKickForTurn: Executing holdTurn cmd");
		dribbleInsecure=true;
		return holdturn->get_cmd(cmd, (dribbleTo-WSinfo::me->pos).ARG());
	}

	POL("getKickForTurn: Slowing ball down is not possible");
	return false;
}

// getTargetsInMe(safest,best)
#define POS_ON_CIRC_NUM  24
void DribbleAround::getTargetsInMe(Vector& safestPos, Vector& bestPos){
	static const float maxOppTackleProb = 0.8;
	float bestDestDist=-1E6;
	float bestVal = -1E6;
	float tmpDist,tmpVal,tackleProb;
	float minDistBallToOpp = 2*ServerOptions::ball_size;
	
	Vector straightAhead;
	straightAhead.init_polar(10,WSinfo::me->ang);
	straightAhead+=nextMeNA.pos;
	Vector tmpVec;

	for(int i=0; i < POS_ON_CIRC_NUM; i++){
		// find the best position in a circle within my kick radius
		tmpVec.init_polar(0.80*WSinfo::me->kick_radius, i*2*PI/POS_ON_CIRC_NUM);
		tmpVec += nextMeNA.pos;
		//DRAW(C2D(tmpVec.x,tmpVec.y,0.1,"black"));
		tmpDist = (nextOppToMe.pos-tmpVec).norm() - nextOppToMe.kick_radius;
		tackleProb = Tools::get_tackle_success_probability(nextOppToMe.pos,tmpVec,nextOppToMe.ang.get_value());
		tmpVal = (tmpVec-straightAhead).norm()  // prefer playing ahead
			- ((tackleProb>maxOppTackleProb)?2*tackleProb:0)
			- ((tmpDist<minDistBallToOpp)?2*tmpDist:0);

		if(tmpDist > bestDestDist){
			bestDestDist = tmpDist;
			safestPos = tmpVec;
		}

		if(tmpVal  > bestVal ){
			// ball is far from opponent,
			// gets better rating than previous best ball
			// and tackling isn't too easy for worstcase-to-me-opp
			bestVal = tmpVal;
			bestPos = tmpVec;
		}
	}
}

// getPlayerDistToBallTraj {{{1
float DribbleAround::getPlayerDistToBallTraj(const PPlayer& p, const Vector& v, int& steps){
  Vector ballPos   = WSinfo::ball->pos;
  Vector ballVel   = v;
	Vector playerPos = p->pos+p->vel;
  float lastDist=1000;
  float pDist;
  float bestpDist=1000;
  for(int i=0;i<30;i++){
    pDist = playerPos.distance(ballPos);
    if(pDist>lastDist) break;
    if(pDist<bestpDist){
      bestpDist = pDist;
			steps = i;
    }
    ballVel = ServerOptions::ball_decay * ballVel;
    ballPos = ballPos + ballVel;
  }
  return bestpDist;
}

// getGoalieKickCmd() {{{2
bool DribbleAround::getGoalieKickCmd(Cmd& cmd){

	bool closeToGoal =      WSinfo::me->pos.x>FIELD_BORDER_X-10 
		              && fabs(WSinfo::me->pos.y)<1.5*ServerOptions::goal_width;
	if(!closeToGoal) return false;

	float toLeftGoalCorner = Tools::my_angle_to(Vector(FIELD_BORDER_X,0.45*ServerOptions::goal_width)).get_value_mPI_pPI();
	float toRightGoalCorner = Tools::my_angle_to(Vector(FIELD_BORDER_X,-0.45*ServerOptions::goal_width)).get_value_mPI_pPI();
	bool lookingAtGoal = toLeftGoalCorner>0 && toRightGoalCorner<0;
	bool goalBehindMe = toLeftGoalCorner<0 && toRightGoalCorner>0;

	// Can I tackle ball into goal? {{{
	if(lookingAtGoal||goalBehindMe){ 
		ANGLE kickDir(WSinfo::me->ang + ANGLE(lookingAtGoal?0:M_PI));
		bool lineIsFree = true;
		WSpset opps = WSinfo::valid_opponents;
		Vector ballVel;
		ballVel.init_polar(ServerOptions::ball_speed_max,WSinfo::me->ang);
		float pDist;
		int steps;
		for(int i=0; i<opps.num; i++){
			pDist = getPlayerDistToBallTraj(opps[i],ballVel,steps);
			if(pDist < opps[i]->kick_radius + steps*opps[i]->speed_max){
				lineIsFree = false;
				break;
			}
			if(opps[i]->number == WSinfo::ws->his_goalie_number){
				if(pDist < ServerOptions::catchable_area_l){
					lineIsFree = false;
					break;
				}
			}
		} 

		if(lineIsFree){
			basic_cmd->set_tackle(lookingAtGoal?100:-100);
			basic_cmd->get_cmd(cmd);
			return true;
		}

	} // }}}
  
  return false;
}

// getKickDestForBallPosInNCycles() {{{1
Vector DribbleAround::getKickDestForBallPosInNCycles(const Vector& target, const int cyc, bool& tooFar){
  Vector vToTarget = target - WSinfo::ball->pos;
  float divBy = 1;
  for(int i=1;i<=cyc; i++){
    divBy += pow(i,ServerOptions::ball_decay, ServerOptions::ball_decay);
  }
  vToTarget.normalize(vToTarget.norm()/divBy);
  if(vToTarget.norm() > ServerOptions::ball_speed_max){
    vToTarget.normalize(ServerOptions::ball_speed_max);
    tooFar = true;
  }else
    tooFar = false;
  return vToTarget + WSinfo::ball->pos;
}

// public setters {{{1
void DribbleAround::set_target(const Vector& dribto){
	dribbleTo = dribto;
}
void DribbleAround::set_keepBall(bool keepBall){
	keepBallSafe = keepBall;
}
void DribbleAround::set_max_speed(int ms){
	maxSpeed = ms;
}

void DribbleAround::setDribbled(bool b){
	didDribble=b;
}

void DribbleAround::resetRequest(){
	POL("Requests resetted! was:"<<request);
	setRequest(DAREQ_NONE);
}

bool DribbleAround::isDribbleInsecure(){
	return dribbleInsecure;
}

// ~DribbleAround() {{{1
DribbleAround::~DribbleAround() {
	delete basic_cmd;
	delete go2pos;
	delete dribble_straight;
	delete onestepkick;
	delete intercept;
	delete holdturn;
}

// PlayerState/BallState stuff {{{1
void DribbleAround::PlayerState::setAssumeNoAction(const PPlayer p, const Vector& target){
	vel = p->vel;
	pos = p->pos + vel;
	age = WSinfo::ws->time - p->time;
	vel *= ServerOptions::player_decay; // TODO: should be needed, but makes strange errors

	ang = p->ang;
	if(p->number == WSinfo::ws->his_goalie_number)
		kick_radius = 1.2*ServerOptions::catchable_area_l; // play it safe: just hold the ball to pass to others!?
	else
		kick_radius = p->kick_radius;
	float newDistanceToPos = (pos - target).norm();
	reachesPos = (kick_radius >= newDistanceToPos);
	movedToAvoidCollision = false;
}

void DribbleAround::PlayerState::setAssumeToPos(const PPlayer p,const Vector& target){
	float distToPos = (p->pos - target).norm();

	// see intercept_ball_bms.c, only valid for distances < 10m.
	bool hasToTurn = (fabs((p->ang - (target-p->pos).ARG()).get_value_mPI_pPI())>asin(1/distToPos));
	bool angleOld = p->age_ang > 0;

	if(hasToTurn && !angleOld){
		setAssumeNoAction(p,target);

		// calculate new angle, assuming full turn towards me
		float toOpp = Tools::my_angle_to(pos).get_value_mPI_pPI();
		ang.set_value(toOpp + (toOpp<0) ? PI : -PI);
	}else{
		setAssumeNoAction(p,target); // law of inertia ;-)
		if(angleOld)
			ang = (target - p->pos).ARG();
		Vector noActionPos = pos;
		Vector a(cos(ang),sin(ang)); // has length 1
		float edp = p->dash_power_rate * p->effort * 100; // assume full speed
		a *= edp;
		if(a.norm() > p->speed_max) a.normalize(p->speed_max);
		Vector lot;
		Geometry2d::projection_to_line(lot, target, Line2d(noActionPos,a));
		float distToLot = (lot-noActionPos).norm();
		bool movesBackwards;
		if(a.norm()>distToLot){
			pos = lot;
			// TODO: vel??
		}else{
			Vector pos1 = noActionPos+a; // ahead
			Vector pos2 = noActionPos-a; // backwards
			movesBackwards = (pos1.sqr_distance(target) > pos2.sqr_distance(target));
			pos = movesBackwards?pos2:pos1;
			// TODO: vel??
		}

		float hisMaxSpeedSqr = SQR(p->speed_max);
		a.normalize(0.1);
		int num=0;
		while(p->pos.sqr_distance(pos) > hisMaxSpeedSqr && num++<20){
			pos += movesBackwards?a:-1*a;
		}
		/*
		 * works very good, but makes agent too conservative:
		 * ball stays in kick_radius too long
		 */
		static const float playersMinDist=2.5*ServerOptions::player_size;
		if(pos.distance(target)<playersMinDist){
			movedToAvoidCollision = true;
			// can reach pos completely, but will probably 
			// try to avoid collision

			// this is a little ugly:
			// assume that the player does not want to "switch sides" in one step
			// (i.e. go to opposite side of object)
			bool switchesSides = fabs((p->pos-target).ANGLE_to(pos-target).get_value_mPI_pPI())>.8*PI;
			switchesSides=false;

			Vector toTar;
			toTar = target - noActionPos;
			pos=target;                                      // all the way
			toTar.normalize(playersMinDist);                 // min dist

			if(!switchesSides)
				pos -= toTar;                                    // "step back"
			else
				pos += toTar;
		}
		/**/
	}
	
	// kick_radius = p->kick_radius; // is set in assumeNA, taking into account goalie catch radius
	float newDistanceToPos = (pos - target).norm();
	reachesPos = (kick_radius >= newDistanceToPos);
}

void DribbleAround::PlayerState::setAssumeToPos(const PPlayer p,const WS::Ball* b){
	setAssumeToPos(p,b->pos);
}
void DribbleAround::PlayerState::setAssumeToPos(const PPlayer p,const BallState& b){
	setAssumeToPos(p,b.pos);
}
void DribbleAround::PlayerState::setAssumeToPos(const PPlayer p,const PlayerState& t){
	setAssumeToPos(p,t.pos);
}
void DribbleAround::PlayerState::setAssumeNoAction(const PPlayer p,const BallState& b){
	setAssumeNoAction(p,b.pos);
}


void DribbleAround::BallState::setAssumeNoAction(const WS::Ball* b){
	vel = b->vel;
	vel *= ServerOptions::ball_decay;
	pos = b->pos + vel;
}