crossposplan.cpp

2006年世界杯足球赛2D仿真组第16名的源代码。在此代码上随便改改

/*
 *  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, Sassan Haradji
 *
 *  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 <Basics.h>
#include <InterceptCalculate.h>
#include <cmath>
#include <CrossPosPlan.h>
#include <Line.h>
#include <map>
#include <algorithm>
#include <Degree.h>
#include <Types.h>
#include <sstream>
#include <cassert>

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

CrossPosPlan::CrossPosPlan(const WorldModel *wm, Form &form,
		const Library &library, Side side):
		Plan("CrossPos", wm, form, library), side(side), updatedRoles(0)
    
{
    config.add("CrossPos");
    config["CrossPos"].setByFile("./Configs/CrossPos.conf");
    LOG << "Loading cross positioning factors from conf..." << endl;
}

CrossPosPlan::~CrossPosPlan()
{
}


void CrossPosPlan::decide()
{
	LOG << "CrossPosPlan::decide" << endl;
	updatedRoles++;

	if (wm->isBallKickable())
	{
		updateRoles();
	}
	else
	{
    try {
        updateRoles();
        if (myRole == Invalid) {
            LOG << "namanaDERROR!" << endl;
            assert (0);
        }
        Vector corrVec = 	preserveShapeUpdate(myRole);
        throw (corrVec + wm->getBody().getPos()).asPoint();
    }
    catch (Point point)
    {
        LOG << "sending command..." << endl;
        Command *command = library.positioning.getPositioningCommand(point);
        command->setCreator(AT_OFFENSE);

        throw command;
    }
	}

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

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

bool CrossPosPlan::isFinished()
{
  if (!isInPlayers(wm->getBody().getUniNum(), "234AB"))
    return true;
    if(!wm->isBallKickable() &&
       library.gwSelection == WOBS_OFFENSE &&
       abs(wm->getBall().getPos().getY()) > 16)
        return false;
    return true;
}

void CrossPosPlan::updateRoles()
{
	RoleType tmpType;
	if (updatedRoles > 10) updatedRoles = 0;
	string hearString = wm->getHearedPlanInfo();
	if (wm->getBody().getUniNum() == wm->getNearestTmmToBall().getUniNum())
	{
		form.sayForm.planSay = true;
		form.sayForm.planHeader = "X";
		form.sayForm.planInfo = "C";
	}
	if (wm->getBody().getUniNum() == wm->getNearestTmmToBall().getUniNum() ||
			hearString.substr(0, 2) != "XC")
	{
		if (updatedRoles > 3)
		{
			for (unsigned i = 0; i < ROLE_NUM; ++i)
				form.sayForm.planInfo += uniNumToChar(roles[RoleType(i)]->getUniNum());
			for (unsigned i = 0 ; i < ROLE_NUM; ++i)
				if (roles.find(RoleType(i)) != roles.end())
					if (roles[RoleType(i)])
					{
						tmpType = RoleType(i);
						LOG << tmpType << " : " << uniNumToChar(roles[RoleType(i)]->getUniNum())
								<< endl;
					}
			return;
		}
		string playerLists[2][5];
		playerLists[0][0] = "A234B";
		playerLists[1][0] = "B432A";
		playerLists[0][1] = "2A34B";
		playerLists[1][1] = "4B32A";
		playerLists[0][2] = "B234A";
		playerLists[1][2] = "A432B";
		playerLists[0][3] = "3B24A";
		playerLists[1][3] = "3A42B";
		playerLists[0][4] = "4B32A";
		playerLists[1][4] = "2A34B";
		roles.clear();
		string busyPlayers = "156789";
		const	Player* tmpPlayer = NULL;
		tmpPlayer = wm->getNearestTmmToPointFromList(wm->getBall().getPos(),
			"123456789AB", busyPlayers + "3");
		if (tmpPlayer || !roles[Crosser]->isValid())
			roles[Crosser] = tmpPlayer;
		else if (!roles[Crosser])
			roles[Crosser] = &wm->getFullPlayer(TID_TEAMMATE, firstPlayerInList(
					playerLists[side][0], busyPlayers) - 1);

		if (roles[Crosser]){
			busyPlayers += uniNumToChar(roles[Crosser]->getUniNum());
			form.sayForm.planInfo += uniNumToChar(roles[Crosser]->getUniNum());
		}

		float minDistance = 1000;
		float tmpDistance = 0;
		const Player* bestPlayer = NULL;

		float oval_a = .6, oval_b = .45;
		for (unsigned i = 0; i < FULL_PLAYERS_NUM; i++)
			if (wm->getFullPlayer(TID_TEAMMATE, i).isValid() &&
	  		!isInPlayers(wm->getFullPlayer(TID_TEAMMATE, i).getUniNum(),
						busyPlayers.c_str()))
				{
					float tmmPlayerX = wm->getFullPlayer(TID_TEAMMATE, i).getPos().getX() -
							roles[Crosser]->getPos().getX();
					float tmmPlayerY = wm->getFullPlayer(TID_TEAMMATE, i).getPos().getY() -
							roles[Crosser]->getPos().getY();
					if (side == SI_LEFT)
						tmmPlayerY = -tmmPlayerY;
					tmpDistance = abs(2 * oval_a *
							tmmPlayerX * tmmPlayerY *	sqrt(abs(
							1 / (oval_a * tmmPlayerY) + 1 / (oval_b * tmmPlayerX))));
					if (tmpDistance < minDistance)
					{
						minDistance = tmpDistance;
						bestPlayer = &wm->getFullPlayer(TID_TEAMMATE, i);
					}
				}

		if (bestPlayer)
			roles[CrosserFriend] = bestPlayer;
		else if (!roles[CrosserFriend])
			roles[CrosserFriend] = &wm->getFullPlayer(TID_TEAMMATE, firstPlayerInList(
					playerLists[side][1], busyPlayers) - 1);

		if (roles[CrosserFriend])
		{
			busyPlayers += uniNumToChar(roles[CrosserFriend]->getUniNum());
			form.sayForm.planInfo += uniNumToChar(roles[CrosserFriend]->getUniNum());
		}
		
		float ballImportance =
				config["CrossPos"]["HomePos"]["BallImportance"].asFloat();
		for (unsigned i = 2; i < ROLE_NUM; ++i)
		{
			if (!roles[RoleType(i)])
			{
				roles[RoleType(i)] =
						wm->getNearestTmmToPointFromList((wm->getBall().getPos() /
						ballImportance) +
						Vector(Point(config["CrossPos"]["HomePos"]["RFX"][i].asFloat(),
						((side == SI_LEFT) * 2 - 1) *
						config["CrossPos"]["HomePos"]["RFY"][i].asFloat())) /
						(1.0 + 1.0 / ballImportance), "123456789AB", busyPlayers);
			}
			if (!roles[RoleType(i)])
				roles[RoleType(i)] = &wm->getFullPlayer(TID_TEAMMATE, firstPlayerInList(
						playerLists[side][i], busyPlayers) - 1);
			if (roles[RoleType(i)])
			{
				busyPlayers += uniNumToChar(roles[RoleType(i)]->getUniNum());
				form.sayForm.planInfo = uniNumToChar(roles[RoleType(i)]->getUniNum());
			}
		}
	}
	else
	{
		/*if (hearString.substr(0, 2) != "XC")
		{
			hearString = "XC";
			if (side == SI_LEFT &&
					wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "2A"))
			{
				hearString += uniNumToChar(wm->getNearestTmmToPointFromList(
						wm->getBall().getPos(), "2A")->getUniNum());
				hearString += uniNumToChar(-(wm->getNearestTmmToPointFromList(
						wm->getBall().getPos(), "2A")->getUniNum() - 6) + 6);
				hearString += "B34";
			}
			else if (side == SI_RIGHT &&
					wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "4B"))
			{
				hearString += uniNumToChar(wm->getNearestTmmToPointFromList(
						wm->getBall().getPos(), "4B")->getUniNum());
				hearString += uniNumToChar(-(wm->getNearestTmmToPointFromList(
						wm->getBall().getPos(), "4B")->getUniNum() - 7) + 8);
				hearString += "A32";
			}
			else if (side == SI_LEFT &&
					!wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "2A"))
				hearString += "2AB34";
			else if (side == SI_RIGHT &&
					!wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "4B"))
				hearString += "4BA32";
		}*/
		if (hearString.substr(0, 2) == "XC")
		{
			if (wm->getFullPlayer(TID_TEAMMATE,
					charToUniNum(hearString[2]) - 1).isValid())
				roles[Crosser] = &wm->getFullPlayer(TID_TEAMMATE, 
						charToUniNum(hearString[2]) - 1);
			if (wm->getFullPlayer(TID_TEAMMATE,
					charToUniNum(hearString[3]) - 1).isValid())
				roles[CrosserFriend] = &wm->getFullPlayer(TID_TEAMMATE, 
						charToUniNum(hearString[3]) - 1);
			if (wm->getFullPlayer(TID_TEAMMATE,
					charToUniNum(hearString[4]) - 1).isValid())
				roles[Supporter] = &wm->getFullPlayer(TID_TEAMMATE, 
						charToUniNum(hearString[4]) - 1);
	 		if (wm->getFullPlayer(TID_TEAMMATE,
					charToUniNum(hearString[5]) - 1).isValid())
				roles[Insider] = &wm->getFullPlayer(TID_TEAMMATE, 
						charToUniNum(hearString[5]) - 1);
			if (wm->getFullPlayer(TID_TEAMMATE,
					charToUniNum(hearString[6]) - 1).isValid())
				roles[Outsider] = &wm->getFullPlayer(TID_TEAMMATE, 
						charToUniNum(hearString[6]) - 1);
		}
	}

	myRole = Invalid;
	for (unsigned i = 0 ; i < ROLE_NUM; ++i)
	{
		if (roles.find(RoleType(i)) != roles.end())
			if (roles[RoleType(i)])
				if (roles[RoleType(i)]->getUniNum() == wm->getBody().getUniNum())
					myRole = RoleType(i);
		if (roles.find(RoleType(i)) != roles.end())
			if (roles[RoleType(i)])
			{
				tmpType = RoleType(i);
				LOG << tmpType << " : " << uniNumToChar(roles[RoleType(i)]->getUniNum())
						<< endl;
			}
	}
}

const Vector CrossPosPlan::preserveShapeUpdate(RoleType role) 
{
    ostringstream ss;
    ss << "RF" << (int)role;
    Vector totalCorrectionVector;
    for (unsigned i = 0 ; i < ROLE_NUM + 1; ++i){
        Vector requiredPos;
				if (RoleType(i) == ConstantPoint) {
					requiredPos.setAsCartesian(0,0);
				} 
				else {
	        if (side == SI_LEFT) {
  	          requiredPos.setAsCartesian(
    	            config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat(),
    		        	config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat());
	        } else {
	            requiredPos.setAsCartesian(
  	              config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat(),
    			        -config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat());
	        }
				}
        Vector currentPos;
        if (roles.find(RoleType(i)) != roles.end()) {
            if (roles[RoleType(i)]) {
							if(RoleType(i) == ConstantPoint) {
			
								Point constPoint;
				        if (side == SI_LEFT) {
  	  			        constPoint. x =
    	      			      config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat();
										constPoint.y =												
    		        				config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat();
				        } else {
										constPoint.x = 
  	        			      config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat();
										constPoint.y =
    			        			-config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat();
				        }
								currentPos.setByPoints(constPoint, wm->getBody().getPos());	
							}
							else {
                currentPos.setByPoints(roles[RoleType(i)]->getPos(),
                                       wm->getBody().getPos());
							}
  	      	float extraLength = abs(requiredPos.getMagnitude() - currentPos.getMagnitude());
                float extraAngle  = abs(requiredPos.getDirection() - currentPos.getDirection());
                LOG << "extra: (R,theta) (" << extraLength << ", "<<
                    extraAngle << ')' << endl;
                Vector correctionVector = requiredPos - currentPos;
                correctionVector.normalize();
      	  	float correctionFactor =
                    extraLength * config["CrossPos"]["RelativePos"][ss.str()+"kLength"][i].asFloat() +
                    extraAngle  * config["CrossPos"]["RelativePos"][ss.str()+"kAngle"][i].asFloat();
                correctionVector *= correctionFactor;
                totalCorrectionVector += correctionVector;
            } else {
                LOG << "Warning.... Cannot find role num " <<  i << endl;
            }
        }
        else {
            LOG << "Warning... Cannot find in map:" << i << endl;
        }
        
    }
    LOG << "..... Final Log for totalCorrectionVector..." << totalCorrectionVector
        << endl;
		DRAW << "DRAW LINE " << 50+wm->getBody().getUniNum() << ' ' << wm->getBody().getPos().getX() << ' ' << wm->getBody().getPos().getY()
					<< " 0 0" << endl;
		DRAW << "END OF DRXING" << endl;
    return totalCorrectionVector;
}