goalstraightposplan.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, 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 <Basics.h>
#include <InterceptCalculate.h>
#include <cmath>
#include <GoalStraightPosPlan.h>
#include <Line.h>
#include <map>
#include <algorithm>
#include <Degree.h>
#include <BasicDash.h>
#include <BasicTurn.h>
#include <cassert>

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


#define LOG_INDIVIDUAL_POINT 0
#define MAX_PATH_POINTS 20
#define PATH_ABS_ANGLE 70

GoalStraightPosPlan::GoalStraightPosPlan(const WorldModel *wm, Form &form,
                                         const Library &library): Plan("GoalPos", wm, form, library),
																																	virtualPass(wm)
{
		config.add("GoalStraightPos");
		config["GoalStraightPos"].setByFile("./Configs/GoalStraightPos.conf");
		LOG << "Loading goal positioning factors from conf..." << endl;
		setPassingPaths();
}


void
GoalStraightPosPlan::setPassingPaths() 
{
		// passing paths..
		LOG << "setting passing paths..." << endl;
		passingPaths[make_pair(10,2)] = config["GoalStraightPos"]["PassingPaths"]["Values"][0].asFloat();
		passingPaths[make_pair(2,10)] = config["GoalStraightPos"]["PassingPaths"]["Values"][1].asFloat();
		passingPaths[make_pair(2,3)]  = config["GoalStraightPos"]["PassingPaths"]["Values"][2].asFloat();
		passingPaths[make_pair(3,2)]  = config["GoalStraightPos"]["PassingPaths"]["Values"][3].asFloat();
		passingPaths[make_pair(3,4)]  = config["GoalStraightPos"]["PassingPaths"]["Values"][4].asFloat();
		passingPaths[make_pair(4,3)]  = config["GoalStraightPos"]["PassingPaths"]["Values"][5].asFloat();
		passingPaths[make_pair(2,4)]  = config["GoalStraightPos"]["PassingPaths"]["Values"][6].asFloat();
		passingPaths[make_pair(4,2)]  = config["GoalStraightPos"]["PassingPaths"]["Values"][7].asFloat();
		passingPaths[make_pair(4,11)] = config["GoalStraightPos"]["PassingPaths"]["Values"][8].asFloat();
		passingPaths[make_pair(11,4)] = config["GoalStraightPos"]["PassingPaths"]["Values"][9].asFloat();
		passingPaths[make_pair(10,3)] = config["GoalStraightPos"]["PassingPaths"]["Values"][10].asFloat();				
		passingPaths[make_pair(3,10)] = config["GoalStraightPos"]["PassingPaths"]["Values"][11].asFloat();
		passingPaths[make_pair(11,3)] = config["GoalStraightPos"]["PassingPaths"]["Values"][12].asFloat();				
		passingPaths[make_pair(3,11)] = config["GoalStraightPos"]["PassingPaths"]["Values"][13].asFloat();
		passingPaths[make_pair(10,11)]= config["GoalStraightPos"]["PassingPaths"]["Values"][14].asFloat();				
		passingPaths[make_pair(11,10)]= config["GoalStraightPos"]["PassingPaths"]["Values"][15].asFloat();				
}



GoalStraightPosPlan::~GoalStraightPosPlan()
{
}


void
GoalStraightPosPlan::decide()
{
		LOG << "GoalStraightPosPlan::decide" <<endl;
		if ((isInPlayers(wm->getBody().getUniNum(), "A") &&
				 wm->getHearedPlanStatus() == 1) ||
        (isInPlayers(wm->getBody().getUniNum(), "B") &&
         wm->getHearedPlanStatus() == 2))
        virtualPass.manager(form, library);
		LOG << "GoalStraightPosPlan::after some lines" <<endl;
		Point point = wm->getBody().getPos().asPoint();
		try {
				Point homePoint = library.positioning.getHomePoint(wm->getBody().getUniNum());
				LOG << "Home Point is : " << homePoint << endl;
				if (wm->getBall().getPos().getX() < 44)
				{
					if (isInPlayers(wm->getBody().getUniNum() , "AB")) {
						homePoint.y *= config["GoalStraightPos"]["BasicGeometry"]["ABYFactor"].asFloat();	
						if (homePoint.y > 30)
								homePoint.y = 30;
						if (homePoint.y < -30)
								homePoint.y = -30;
						throw homePoint;
					}
				}
				
				if (wm->getBody().getDistance(homePoint) > 7.5) {
						LOG << "going to home point..." << endl;
						throw homePoint;
				}
				LOG << "oppOffsideLine.x : " << wm->getOppOffsideLine(true) << endl;
				if (wm->getOppOffsideLine(true) < wm->getBody().getPos().getX()) {
						LOG << "warning...  running away from offside..." << endl;
						throw Point(wm->getOppOffsideLine(true) - 1, wm->getBody().getPos().getY());
				}
				if (wm->getBody().getDistance(wm->getBall()) > 15 && 
					getProperAngle(wm->getBody().getBodyDir(), 0) > PATH_ABS_ANGLE)
				{
					LOG << "urgent turn to goal:1" << endl;
					turnToGoal();
				}
				if (wm->getBody().getDistance(wm->getBall()) > 5 && 
					getProperAngle(wm->getBody().getBodyDir(), 0) > 100)
				{
					LOG << "urgent urgent turn to goal: 2" << endl;
					turnToGoal();
				}
				Vector moveDir = getBestVectorInFreeArea();
				if (moveDir.getMagnitude() == 0) {
					turnToGoal();
				} else {
					moveDir += wm->getBody().getPos();
					throw moveDir.asPoint();
				}
    }
    catch (Point point)
    {
		bool posOnlyFlag = false;
		if (wm->getBallStatus() == BS_FREE_BALL_TMM || wm->getBallStatus() == BS_KICKABLE_TMM_OPP)
		{
			if (wm->getCurInterCalculate().existFastestPlayer()) {
				LOG << "Fastest OK ... " << endl;
				LOG << "P:" << wm->getCurInterCalculate().getFastestInterPlayer().getInterPoint() << endl;
				if (wm->getCurInterCalculate().getFastestInterPlayer().getInterPoint().x < 35)
					posOnlyFlag = true;
			}
		}
		if (wm->getOppOffsideLine(true) < point.x) {
			LOG << "not going to offside...." << endl;
			turnToGoal();
		}
		Command * command = NULL;
        LOG << "sending command..." << endl;
		if (point.getDistance(wm->getBody().getPos().asPoint()) > 5.0  || posOnlyFlag) {
			LOG << "-----------------> Positioning " << endl;
			command =  library.positioning.getPositioningCommand(point);
		} else {
			LOG << "--------------> PGTP" << endl;
			PreciseGotoPoint pgtp(AT_OFFENSE, point, 0.5, wm->getBody(), true, 20);
			command = pgtp.getCommand();
		}
		if (!command) 
			command = new EmptyCommand(AT_OFFENSE);
        command->setCreator(AT_OFFENSE);
        throw command;
    }

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


float
GoalStraightPosPlan::getValueForPoint(const Point & calcPoint) 
{
		float goalieDist = -1;
//		float dist = 0;
		Vector v1, v2, v;
#ifdef LOG_INDIVIDUAL_POINT
		LOG << "Calc point:: " << calcPoint.x << ' ' << calcPoint.y;
#endif
		v1.setByPoints(calcPoint, Point(52.5, +6.3));
		v2.setByPoints(calcPoint, Point(52.5, -6.3));
		v.setByPoints(calcPoint, calcPoint);
		bool goalieFound = false;
		Player ballOwner = wm->getNearestTmmToBall();

		for (int i =0 ; i <  HALF_PLAYERS_NUM; ++i)
				if (wm->getHalfPlayer(TID_OPPONENT, i).isGoalie()) {
						goalieFound = true;
						v.setByPoints (calcPoint, wm->getHalfPlayer(TID_OPPONENT,
																												i).getPos().asPoint());
				}
		for (int i =0 ; i <  FULL_PLAYERS_NUM; ++i)
				if (wm->getFullPlayer(TID_OPPONENT, i).isGoalie()) {
						goalieFound = true;
						v.setByPoints (calcPoint, wm->getFullPlayer(TID_OPPONENT,
																												i).getPos().asPoint());
				}
		LOG << " golieFound " << goalieFound;
		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;