headdecision.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: Ahmad Boorghany
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>
#include <cassert>

#include <Basics.h>
#include <Degree.h>
#include <Logger.h>
#include <Command.h>
#include <HeadDecision.h>
#include <AdvancedAgent.h>


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


// HeadDecisionForm's functions
void HeadDecisionForm::reset()
{
	tnMode = TNM_LOOK_NORMALLY;
}


// HeadDecision's functions
HeadDecision::HeadDecision(const AdvancedAgent *owner):
		owner(owner)
{
	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
		viewPartObjectsNums[i] = 0;

	turnNeckCommand = NULL;
	changeViewCommand = NULL;

	worldModel = &owner->getWorldModel();
}

HeadDecision::~HeadDecision()
{
}

Command *HeadDecision::getTurnNeckCommand()
{
	return turnNeckCommand;
}

Command *HeadDecision::getChangeViewCommand()
{
	return changeViewCommand;
}

void HeadDecision::decide(const Command *bodyCycleCommand,
		const HeadDecisionForm &headForm)
{
	float nextViewAngle = 0;
	ViewModeWidth nextWidth = VMW_NARROW;
	TurnNeckMode tnMode = headForm.tnMode;

	LOG << "HeadDecision::decide" << endl;
	LOG << "TurnNeckMode: " << headForm.tnMode << endl;

	nextBody = worldModel->getBody();
	nextBody.simulateByAction(bodyCycleCommand);
	nextBody.simulateByDynamics();

	nextBall = worldModel->getBall();
	nextBall.simulateByAction(worldModel->getBody(), bodyCycleCommand);
	nextBall.simulateByDynamics(nextBody, 1,
			worldModel->getBallStatus(), worldModel->getCurTime());

	if (tnMode == TNM_AUTO_PLAY_ON)
	{
		if (worldModel->getBall().getAbsVec().getMagnitude() > 7.5)
			tnMode = TNM_LOOK_NORMALLY;
		else
			tnMode = TNM_LOOK_CAREFULLY;
	}

	if (nextBall.getAbsVec().getMagnitude() <
			worldModel->getBody().getVisibleDistance())
	{
		if (tnMode == TNM_LOOK_CAREFULLY_TO_BALL ||
			tnMode == TNM_LOOK_NORMALLY_TO_BALL)
			tnMode = TNM_LOOK_CAREFULLY;
	}

	assert(tnMode != TNM_NONE);
	TurnNeckMode lastMode = TNM_NONE;

	while (lastMode != tnMode)
	{
		lastMode = tnMode;

		switch (tnMode)
		{
			case TNM_LOOK_NORMALLY:
				nextWidth = owner->getSyncedNormal();
				nextViewAngle = worldModel->getBody().viewWidthToViewAngle(nextWidth);
				setWeightsForLookNormally();
				break;

			case TNM_LOOK_CAREFULLY:
				nextWidth = owner->getSyncedNarrow();
				nextViewAngle = worldModel->getBody().viewWidthToViewAngle(nextWidth);
				setWeightsForLookCarefully();
				break;

			case TNM_LOOK_CAREFULLY_TO_BALL:
				nextWidth = owner->getSyncedNarrow();
				nextViewAngle = worldModel->getBody().viewWidthToViewAngle(nextWidth);
				decideForLookCarefullyToBall();
				break;

			case TNM_LOOK_NORMALLY_TO_BALL:
				nextWidth = owner->getSyncedNormal();
				nextViewAngle = worldModel->getBody().viewWidthToViewAngle(nextWidth);
				decideForLookNormallyToBall(nextViewAngle, tnMode);
				break;

			case TNM_LOOK_CAREFULLY_TO_OPP_GOALIE:
				nextWidth = owner->getSyncedNarrow();
				nextViewAngle = worldModel->getBody().viewWidthToViewAngle(nextWidth);
				decideForLookCarefullyToOppGoalie(tnMode);
				break;

			default:
				assert(0);
		}
	}

	changeViewCommand = new ChangeViewCommand(AT_HEAD_DECISION, VMQ_HIGH, nextWidth);

	if (tnMode == TNM_LOOK_NORMALLY ||
		tnMode == TNM_LOOK_CAREFULLY)
		decideForTurnNeckCommand(nextViewAngle);

	logVariables();
}

void HeadDecision::decideForLookCarefullyToBall()
{
	LOG << "HeadDecision::decideForLookCarefullyToBall" << endl;

	float dir = normalizeAngle(nextBall.getHeadVec().getDirection());
	turnNeckCommand = new TurnNeckCommand(AT_HEAD_DECISION, dir);
}

void HeadDecision::decideForLookCarefullyToOppGoalie(TurnNeckMode &tnMode)
{
	LOG << "HeadDecision::decideForLookCarefullyToOppGoalie" << endl;

	float dir;
	Player nextOppGoalie;

	if (worldModel->getOppGoalie())
	{
		nextOppGoalie = *worldModel->getOppGoalie();
		nextOppGoalie.simulateByDynamics(nextBody);
		dir = nextOppGoalie.getHeadVec().getDirection();
	}
	else
	{
		tnMode = TNM_LOOK_CAREFULLY;
		return;
	}

	dir = normalizeAngle(dir);
	turnNeckCommand = new TurnNeckCommand(AT_HEAD_DECISION, dir);
}

void HeadDecision::decideForLookNormallyToBall(float nextViewAngle, TurnNeckMode &tnMode)
{
	LOG << "HeadDecision::decideForLookNormallyToBall" << endl;

	float dir;

	if (worldModel->getBall().isValid())
	{
		float headDirRight = nextBall.
			getAbsVec().getDirection() + (nextViewAngle - 15);
		float headDirLeft = nextBall.
			getAbsVec().getDirection() - (nextViewAngle - 15);

		if (abs(Degree::getDeltaAngle(nextBody.getBodyDir(), headDirLeft)) < 90 ||
			abs(Degree::getDeltaAngle(nextBody.getBodyDir(), headDirRight)) < 90)
		{
			if (worldModel->getSeeDeltaCycle(headDirRight, 0.2) >
				worldModel->getSeeDeltaCycle(headDirLeft, 0.2))
				dir = headDirRight;
			else
				dir = headDirLeft;
		}
		else
		{
			tnMode = TNM_LOOK_NORMALLY;
			return;
		}

		dir = Degree::getDeltaAngle(dir, nextBody.getBodyDir());
		dir = Degree::getDeltaAngle(dir, nextBody.getRelativeHeadDir());
	}
	else
		dir = -nextBody.getRelativeHeadDir();

	dir = normalizeAngle(dir);
	turnNeckCommand = new TurnNeckCommand(AT_HEAD_DECISION, dir);
}

void HeadDecision::decideForTurnNeckCommand(float nextViewAngle)
{
//	LOG << "HeadDecision::decideForTurnNeckCommand" << endl;

	float bodyHeadDir;
	float turnNeckAngle;
	float turnNeckWeights[(int)(180.0 / TURN_NECK_RESOLUTION) + 1];
	unsigned i, j;

	updateViewPartObjectsNum();
	for (i = 0; i < VIEW_PARTS_NUM; i++)
		viewPartWeights[i] = getViewPartWeight(i);

//LOG << "NextCycleViewAngle:" << nextViewAngle << endl;
	for (i = 0; i <= 180.0 / TURN_NECK_RESOLUTION; i++)
	{
//LOG << "i:" << i << endl;
		turnNeckWeights[i] = 0;
		bodyHeadDir = nextBody.getBodyDir() - 90 + i * TURN_NECK_RESOLUTION;
//LOG << "BodyHeadDir:" << bodyHeadDir << endl;

		for (j = 0; j < VIEW_PARTS_NUM; j++)
		{
			if (worldModel->getSeeViewPartValue(j, bodyHeadDir, nextViewAngle) >= 0.75)
			{
//LOG << "j:" << j << endl;
//LOG << "ViewPartWeight[j]:" << viewPartWeights[j] << endl;
				turnNeckWeights[i] += viewPartWeights[j] * viewPartWeights[j];
			}
		}
//LOG << "TurnNeckWeight[i]:" << turnNeckWeights[i] << endl;
	}

	unsigned turnNeckWeightFlag = 0;
	for (i = 1; i < (unsigned)(180.0 / TURN_NECK_RESOLUTION) + 1; i++)
	{
//LOG << "TurnNeckWeight[" << i << "]:" << turnNeckWeights[i] << endl;
		if (turnNeckWeights[i] > turnNeckWeights[turnNeckWeightFlag])
			turnNeckWeightFlag = i;
	}

//LOG << "TurnNecWeightFlag:" << turnNeckWeightFlag << endl;
	turnNeckAngle = (float)turnNeckWeightFlag * TURN_NECK_RESOLUTION - 90.00;
//LOG << "TurnNeckAngle:" << turnNeckAngle << endl;

	turnNeckCommand = new TurnNeckCommand(AT_HEAD_DECISION, turnNeckAngle -
			worldModel->getBody().getRelativeHeadDir());
//LOG << "TurnNeckCommand.Command:" << turnNeckCommand->toString() << endl;
}

void HeadDecision::logVariables()
{
	LOG << "HeadDecision View Parts:" << endl;
	
	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
		LOG << "\tVP[" << i << "] >"
			<< " Cyc:" << worldModel->getViewPartCycle(i)
			<< " ObN:" << viewPartObjectsNums[i]
			<< " Wei:" << viewPartWeights[i] << endl;
}

void HeadDecision::setWeightsForLookNormally()
{
/*	if (worldModel->getBallStatus() == BS_FREE_BALL ||
		worldModel->getBallStatus() == BS_FREE_BALL_TMM ||
		worldModel->getBallStatus() == BS_FREE_BALL_OPP ||
		worldModel->getBallStatus() == BS_FREE_BALL_TMM_OPP)
		haveBallRate = 1;
	else
		haveBallRate = 7;*/

	haveBallRate = 1.2;
	havePlayerRate = 7;
	objectsNumRate = 0;
}

void HeadDecision::setWeightsForLookCarefully()
{
//	LOG << "HeadDecision::setWeightsForLookCarefully" << endl;

/*	if (worldModel->getBallStatus() == BS_FREE_BALL ||
		worldModel->getBallStatus() == BS_FREE_BALL_TMM ||
		worldModel->getBallStatus() == BS_FREE_BALL_OPP ||
		worldModel->getBallStatus() == BS_FREE_BALL_TMM_OPP)
		haveBallRate = 1;
	else
		haveBallRate = 7;*/

	haveBallRate = 1.2;
	havePlayerRate = 7;
	objectsNumRate = 0;
}

void HeadDecision::updateViewPartObjectsNum()
{
//	LOG << "HeadDecision::updateViewPartObjectsNum" << endl;

	float deltaAngle;
	unsigned i, j;
	
	for (i = 0; i < VIEW_PARTS_NUM; i++)
	{
		viewPartObjectsNums[i] = 0;

		// Counting full players.
		for (j = 0; j < FULL_PLAYERS_NUM; j++)
		{
			if (worldModel->getFullPlayer(TID_TEAMMATE,j).isAlive() &&
				!worldModel->getFullPlayer(TID_TEAMMATE,j).isBody())
			{
				deltaAngle = abs(getDeltaAngle(
					worldModel->getFullPlayer(TID_TEAMMATE,j).getAbsVec().getDirection(),
					worldModel->getViewPartDir(i)));

				if (deltaAngle <= VIEW_PART_SIZE / 2)
					viewPartObjectsNums[i]++;
			}

			if (worldModel->getFullPlayer(TID_OPPONENT,j).isAlive())
			{
				deltaAngle = abs(getDeltaAngle(
					worldModel->getFullPlayer(TID_OPPONENT,j).getAbsVec().getDirection(),
					worldModel->getViewPartDir(i)));

				if (deltaAngle <= VIEW_PART_SIZE / 2)
					viewPartObjectsNums[i]++;
			}
		}

		// Counting half players.
		for (j = 0; j < HALF_PLAYERS_NUM; j++)
		{
			if (worldModel->getHalfPlayer(TID_TEAMMATE,j).isAlive())
			{
				deltaAngle = abs(getDeltaAngle(
					worldModel->getHalfPlayer(TID_TEAMMATE,j).getAbsVec().getDirection(),
					worldModel->getViewPartDir(i)));

				if (deltaAngle <= VIEW_PART_SIZE / 2)
					viewPartObjectsNums[i]++;
			}

			if (worldModel->getHalfPlayer(TID_OPPONENT,j).isAlive())
			{
				deltaAngle = abs(getDeltaAngle(
					worldModel->getHalfPlayer(TID_OPPONENT,j).getAbsVec().getDirection(),
					worldModel->getViewPartDir(i)));

				if (deltaAngle <= VIEW_PART_SIZE / 2)
					viewPartObjectsNums[i]++;
			}
		}

		// Counting quarter players.
		for (j = 0; j < QUARTER_PLAYERS_NUM; j++)
			if (worldModel->getQuarterPlayer(j).isAlive())
			{
				deltaAngle = abs(getDeltaAngle(
					worldModel->getQuarterPlayer(j).getAbsVec().getDirection(),
					worldModel->getViewPartDir(i)));

				if (deltaAngle <= VIEW_PART_SIZE / 2)
					viewPartObjectsNums[i]++;
			}

		// Counting the ball.
		if (nextBall.getAbsVec().getMagnitude() >
				worldModel->getBody().getVisibleDistance())
		{
			deltaAngle = abs(getDeltaAngle(
				nextBall.getAbsVec().getDirection(),
				worldModel->getViewPartDir(i)));

			if (deltaAngle <= VIEW_PART_SIZE / 2)
				viewPartObjectsNums[i]++;
		}

		// Counting the opponent goal
		if (worldModel->getBall().getAbsVec().getMagnitude() < 5 &&
			worldModel->getBody().getPos().getX() > 30)
		{
			Vector vec;

			vec.setByPoints(worldModel->getBody().getPos(), Point(52, 5.5));
			deltaAngle = abs(getDeltaAngle(
				vec.getDirection(), worldModel->getViewPartDir(i)));
			if (deltaAngle <= VIEW_PART_SIZE / 2)
				viewPartObjectsNums[i]++;

			vec.setByPoints(worldModel->getBody().getPos(), Point(52, -5.5));
			deltaAngle = abs(getDeltaAngle(
				vec.getDirection(), worldModel->getViewPartDir(i)));
			if (deltaAngle <= VIEW_PART_SIZE / 2)
				viewPartObjectsNums[i]++;
		}

		// Counting the forward angle
		if (worldModel->getBall().getAbsVec().getMagnitude() < 3 &&
			worldModel->getBody().getPos().getX() < 35)
		{
			Vector vec;

			vec.setAsCartesian(10, 0);
			deltaAngle = abs(getDeltaAngle(
				vec.getDirection(), worldModel->getViewPartDir(i)));
			if (deltaAngle <= VIEW_PART_SIZE / 2)
				viewPartObjectsNums[i]++;
		}
	}
}

float HeadDecision::getViewPartWeight(unsigned viewPartNum)
{
//	LOG << "HeadDecision::getViewPartWeight" << endl;

	float weight;
	float deltaAngle;
	Vector oppGoalUpVec;
	Vector oppGoalDownVec;

	// Dont see weight
	weight = worldModel->getViewPartCycle(viewPartNum) + 1;

	// HaveBall weight
	deltaAngle = abs(getDeltaAngle(worldModel->getViewPartDir(viewPartNum),
		nextBall.getAbsVec().getDirection()));
	if (deltaAngle <= VIEW_PART_SIZE / 2/* || 
		nextBall.getAbsVec().getMagnitude() <=
			worldModel->getBody().getVisibleDistance()*/) // SenseArea
		weight *= haveBallRate;

	// HavePlayer weight
	if (viewPartObjectsNums[viewPartNum] > 0)
		weight *= havePlayerRate;

	// ObjectsNum weight
	weight += viewPartObjectsNums[viewPartNum] * objectsNumRate;

	return weight;
}