goalplan.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

/*
 *  Copyright 2002-2005, Mersad Team, Allameh Helli High School (NODET).
 *
 *  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.
 *
 *  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 Library General Public License for more details.
 *
 *  This file is created by: Darioush Jalali, Mohammad Salehe
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <AdvancedAgent.h>
#include <Logger.h>
#include <cmath>
#include <BasicDash.h>
#include <GoalPlan.h>
#include <ShootPlan.h>
#include <DribblePlan.h>
#include <SRPDribblePlan.h>
#include <PassPlan.h>
#include <InterceptPlan.h>
#include <GoalStraightPosPlan.h>
#include <OffensePosPlan.h>
#include <CrossPlan.h>
#include <Basics.h>
#include <Degree.h>
#include <cassert>

using namespace std;
using namespace Basics;
using namespace Degree;

GoalPlan::GoalPlan(const WorldModel *wm, Form &form, const Library &library):
				Plan("Goal", wm, form, library)
{


		config.add("GoalPlan");
		config["GoalPlan"].setByFile("./Configs/GoalPlan.conf");
		LOG << "Loading goal  from conf..." << endl;
	/*
		30 = very good
		27 = good
		23 = normal
		20 = not good (oh my god)
	*/
	passClasses.reserve(4);
	passClasses.push_back(.30);	
	passClasses.push_back(.27);
	passClasses.push_back(.24);	
	passClasses.push_back(.23);
}

GoalPlan::~GoalPlan()
{
}

void GoalPlan::decide()
{

		LOG << "GoalPlan::decide" << endl;

		if (wm->isBallKickable())
		{
/*				startPlan(new ShootPlan(wm, form, library), .3);
				Plan::decide();	
				vector <string> priorities;
				unsigned uniNum = 0;
				bool AInPassArea = false;
				bool BInPassArea = false;
				uniNum = findBestShooter("234AB");
				if (uniNum)
				{
					string pushP;
					pushP += uniNumToChar(uniNum);
					priorities.push_back(pushP);
				}
				switch (wm->getBody().getUniNum()) {
					case 2:
						if (wm->getBody().getPos().getX() < 36)
						{
							priorities.push_back("A");
							priorities.push_back("3");
							priorities.push_back("4");
						}
						else
						{
							priorities.push_back("3");
							priorities.push_back("4");
							priorities.push_back("A");
						}
//						priorities.push_back("23456789AB");
						break;
					case 4:
						if (wm->getBody().getPos().getX() < 36)
						{
								priorities.push_back("B");
								priorities.push_back("3");
								priorities.push_back("2");
						}
						else
						{
								priorities.push_back("3");
								priorities.push_back("2");
								priorities.push_back("B");
						}
//						priorities.push_back("23456789AB");						
						break;
					case 3:
						uniNum = findBestShooter("24");
						LOG << "Player 3 -> BestShooter(24)=" << uniNum << endl;
						if (uniNum) {
							string firstP;
							firstP += uniNumToChar(uniNum);
							string secondP;
							if (uniNum == 2) 
								secondP = "4";
							else
								secondP = "2";
							priorities.push_back(firstP);
							priorities.push_back(secondP);
						} else 
							priorities.push_back("24");
						if (wm->getFullPlayer(TID_TEAMMATE, 9).isValid())
							if (wm->getFullPlayer(TID_TEAMMATE, 9).getPos().getX() > 36 &&
								abs(wm->getFullPlayer(TID_TEAMMATE, 9).getPos().getY()) < 20)
								AInPassArea = true;
						if (wm->getFullPlayer(TID_TEAMMATE, 10).isValid())
							if (wm->getFullPlayer(TID_TEAMMATE, 10).getPos().getX() > 36 &&
								abs(wm->getFullPlayer(TID_TEAMMATE, 10).getPos().getY()) < 20)
								BInPassArea = true;
						if (AInPassArea && !BInPassArea)
							priorities.push_back("A");
						else if (!AInPassArea && BInPassArea)
							priorities.push_back("B");
						else if (AInPassArea && BInPassArea)
						{
							uniNum = findBestShooter("AB");
							LOG << "Player 3 -> BestShooter(AB)=" << uniNum << endl;
							if (uniNum) {
								string firstP;
								firstP += uniNumToChar(uniNum);
								string secondP;
								if (uniNum == 9) 
									secondP = "B";
								else
									secondP = "A";
								priorities.push_back(firstP);
								priorities.push_back(secondP);
							} else 
								priorities.push_back("AB");
						}
//						priorities.push_back("23456789AB");
						break;
					case 10:
						priorities.push_back("2B");	
						priorities.push_back("3");	
//						priorities.push_back("23456789AB");	
						break;
					case 11:
						priorities.push_back("4A");	
						priorities.push_back("3");	
//						priorities.push_back("23456789AB");	
						break; *

				}
				*
					note: push_back("23456789AB") is COed because of IQlogical problems....
				*
				startPassPlanWithPriorities(passClasses, priorities, true);
				startPlan(bestPlan(3, true, new DribblePlan(wm, form, library),
											 new ShootPlan(wm, form, library),
											 new PassPlan(wm, form, library)), .22);

				startPlan(bestPlan(4, true,
									 new PassPlan(wm, form, library),
									 new ShootPlan(wm, form, library),
									 new DribblePlan(wm, form, library),
									 new SRPDribblePlan(wm, form, library)), 0.15);

				LOG << "Goal Plan WARNING!!! : "
						<< "All WithBall Plans Finished Without Return Any Command." << endl;
				LOG << "Goal Plan : "
						<< "Now Run Dribble Plan Without Check SuccessRate or isFinished"
						<< "(Urgent State)." << endl;

				DribblePlan dribblePlan(wm, form, library);
				dribblePlan.decide();*/


			// Welcome to A&M security tofs
			// Do not change this unless you are passed
			//         OFFICIAL PROCEDURE

			Plan *passPlan = new PassPlan(wm, form, library);
			float passVal = passPlan->successRate();

			Plan *shootPlan = new ShootPlan(wm, form, library);
			float shootVal = shootPlan->successRate();

			Plan *dribblePlan = new DribblePlan(wm, form, library);
			float dribbleVal = dribblePlan->successRate();

			Plan *srpDribblePlan = new SRPDribblePlan(wm, form, library);
			float srpDribbleVal = srpDribblePlan->successRate();

			LOG << "A&M security values: " << endl;
			LOG << "\tshootVal = " << shootVal << endl;
			LOG << "\tpassVal = " << passVal << endl;
			LOG << "\tsrpDribbleVal = " << srpDribbleVal << endl;
			LOG << "\tdribbleVal = " << dribbleVal << endl;

			if (shootVal >= dribbleVal &&
				shootVal >= srpDribbleVal &&
				shootVal >= passVal)
			{ // Shoot
				delete passPlan;
				delete dribblePlan;
				delete srpDribblePlan;

				startPlan(shootPlan);
			}
			else if (passVal >= dribbleVal && passVal >= srpDribbleVal)
			{ // Pass
				delete shootPlan;
				delete dribblePlan;
				delete srpDribblePlan;

				startPlan(passPlan);
			}
			else if (srpDribbleVal >= dribbleVal)
			{ // SRPDribble
				delete passPlan;
				delete shootPlan;
				delete dribblePlan;

				startPlan(srpDribblePlan);
			}
			else
			{ // Dribble
				delete passPlan;
				delete shootPlan;
				delete srpDribblePlan;

				dribblePlan->decide();
			}
		}
		else
		{
			if (!wm->isBallKickable() &&
					library.gwSelection == WOBS_OFFENSE)
			{
				Plan::resetPlans();
				startPlan(new GoalStraightPosPlan(wm, form, library));
			}
		
			Plan::resetPlans();
				if (library.gwSelection == WOBS_INTERCEPT)
					startPlan(new InterceptPlan(wm, form, library));
				startPlan(new OffensePosPlan(wm, form, library));
		}

		LOG << "GoalPlan Now Kill Program" << endl;
		assert(0);

}

double GoalPlan::successRate()
{
		return 1.0;
}

bool GoalPlan::isFinished()
{
	if (wm->getBody().getPos().getX() >= 
			config["Agent"]["PlanningSystem"]["GoalPlanX"].asFloat() - 5)
		return false;
//		if ((wm->getBallStatus() == BS_KICKABLE_OPP) ||
//				(wm->getBallStatus() == BS_FREE_BALL_OPP))
//				return true;
	return true;
}


void
GoalPlan::startPassPlanWithPriorities(const vector <float>& passClasses, const vector <string>& passPriorities,
										bool tryDribble) 
{
	for (unsigned i =0 ; i < passClasses.size(); ++i) {
		LOG << "passClass : " << passClasses[i] << endl;
		for (unsigned j = 0; j < passPriorities.size(); ++j) {
			LOG << "passPriorities : " << passPriorities[j] << endl;
			vector < Plan * > possiblePasses;
			vector < float  > helpPass;
			possiblePasses.reserve(passPriorities[j].size()+tryDribble);
			if (tryDribble)
				helpPass.reserve(passPriorities[j].size()+tryDribble);
			for (unsigned p = 0; p < passPriorities[j].size(); ++p) {
				possiblePasses.push_back( new PassPlan(wm, form, library, BPM_ALL,
												charToUniNum(passPriorities[j][p])));
				helpPass.push_back(0.03);
			}
			if (tryDribble) 
			{
				LOG << "try Dribble" << endl;
				possiblePasses.push_back(new DribblePlan(wm, form, library));
				helpPass.push_back(0.0);
			}
			LOG << "...." << endl;
			startPlan(bestPlan(possiblePasses, helpPass, true),  passClasses[i]);
		}
	}
}



unsigned
GoalPlan::findBestShooter(string inPlayers) 
{
	unsigned retVal = 0;
	float maxValue = -0xFFFF;
	for (unsigned i =0 ; i < inPlayers.size(); ++i) {
		unsigned uniNum = charToUniNum(inPlayers[i]);
		const Player * player = NULL;
		for (int fp = 0 ; fp < FULL_PLAYERS_NUM; ++fp)
			if (wm->getFullPlayer(TID_TEAMMATE, fp).isValid())
				if (wm->getFullPlayer(TID_TEAMMATE, fp).getUniNum() == uniNum)
					player = &(wm->getFullPlayer(TID_TEAMMATE,fp));
		if (!player) continue;
		pair<float,float> weights = getWeights(player);
		float value = config["GoalPlan"]["SelectPasser"]["GoalieDist"].asFloat() * weights.first
					+ config["GoalPlan"]["SelectPasser"]["Alpha"].asFloat() * weights.second;
		if (!retVal || (value>maxValue)) {
			maxValue = value;
			retVal = uniNum;
		}
	}
	return retVal;
}

pair<float,float>
GoalPlan::getWeights(const Player * player) 
{
	pair <float, float> retval;
	float goalieDist = -1;
	Vector v1, v2, v;
	Point calcPoint = player->getPos().asPoint();
	LOG << " cP: " << calcPoint;
	v1.setByPoints(calcPoint, Point(52.5, +6.3));
	v2.setByPoints(calcPoint, Point(52.5, -6.3));
	v.setByPoints(calcPoint, calcPoint);
	bool goalieFound = false;
	for (int i =0 ; i <  HALF_PLAYERS_NUM; ++i)
			if (wm->getHalfPlayer(TID_TEAMMATE, i).isGoalie()) {
					goalieFound = true;
					v.setByPoints (calcPoint, wm->getHalfPlayer(TID_TEAMMATE, i).getPos().asPoint());
			}
	for (int i =0 ; i <  FULL_PLAYERS_NUM; ++i)
			if (wm->getFullPlayer(TID_TEAMMATE, i).isGoalie()) {
					goalieFound = true;
					v.setByPoints (calcPoint, wm->getFullPlayer(TID_TEAMMATE, i).getPos().asPoint());
			}
	LOG << " goalieFound " << goalieFound;
	LOG << " v,v1,v2:" << v << " * " << v1 << "* " << v2;
	float alpha = 0.0;
	float d1 = normalizeAngle(v1.getDirection()),
			d2 = normalizeAngle(v2.getDirection());
	if (goalieFound) {
			float d = normalizeAngle(v.getDirection());
			LOG << "d1: " << d1 << "d2: " << d2 << "d: " << d << endl;				
			if ((d1 >= d) && (d >= d2)) {
					// positioning is OK...
					float pa1 = getProperAngle (d1,d);
					float pa2 = getProperAngle (d,d2);
					if (pa1 > pa2) 
							alpha = pa1;
					else
							alpha = pa2;
			}
			else {
					alpha = getProperAngle(d1,d2);
			}
			goalieDist = v.getMagnitude();
	}
	else {
			LOG << "Warning.. goalie not found. (not isValid checked)" << endl;
			alpha = 0.0;
			goalieDist = 0.0;
	}
	LOG << "alpha: " << alpha << " gd: " << goalieDist <<endl;
	retval.first = goalieDist;
	retval.second = alpha;
	return retval;
}

float
GoalPlan::getProperAngle(float angle1, float angle2) const
{
		return abs(normalizeAngle(angle1-angle2));
}