headdecision.cpp

mersad源码 03年robocup 季军 可以研究一下大家

/*
 *  Copyright 2002-2004, Mersad Team, Allame 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.
 *
 *  Created by: Ahmad Boorghany
 *  Released on Friday 1 April 2005 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;

HeadDecision::HeadDecision(const AdvancedAgent *owner):
		owner(owner)
{
	mode = TNM_LOOK_NORMALLY;

	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)
{
	float nextViewAngle = 0;
	ViewModeWidth nextWidth = VMW_NARROW;

	LOG << "HeadDecision::decide" << 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 (mode == TNM_AUTO_PLAY_ON)
	{
		if (worldModel->getBall().getAbsVec().getMagnitude() > 7.5)
			mode = TNM_LOOK_NORMALLY;
		else
			mode = TNM_LOOK_CAREFULLY;
	}

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

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

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

		switch (mode)
		{
			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);
				break;

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

			default:
				assert(0);
		}
	}

	changeViewCommand = new ChangeViewCommand(VMQ_HIGH, nextWidth);

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

	logVariables();
}

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

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

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

	float dir;
	Player nextOppGoalie;

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

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

void HeadDecision::decideForLookNormallyToBall(float nextViewAngle)
{
	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 (fabs(Degree::getDeltaAngle(nextBody.getBodyDir(), headDirLeft)) < 90 ||
			fabs(Degree::getDeltaAngle(nextBody.getBodyDir(), headDirRight)) < 90)
		{
			if (worldModel->getSeeDeltaCycle(headDirRight, 0.2) >
				worldModel->getSeeDeltaCycle(headDirLeft, 0.2))
				dir = headDirRight;
			else
				dir = headDirLeft;
		}
		else
		{
			mode = 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(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.80)
			{
//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(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;

	havePlayerRate = 7;
	haveOppGoalRate = 2;
	objectsNumRate = 0.15;
}

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;

	havePlayerRate = 7;
	haveOppGoalRate = 2;
	objectsNumRate = 0.10;
}

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 = fabs(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 = fabs(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 = fabs(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 = fabs(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 = fabs(getDeltaAngle(
					worldModel->getQuarterPlayer(j).getAbsVec().getDirection(),
					worldModel->getViewPartDir(i)));

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

		// Counting the ball.
		deltaAngle = fabs(getDeltaAngle(
			nextBall.getAbsVec().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; // Plus one because of the rates for zero.

	// HaveOppGoal weight
	if (worldModel->isPlayerInOppShootArea(worldModel->getBody()))
	{
		oppGoalUpVec.setByPoints(worldModel->getBody().getPos(),
				Point(52.5, 10));
		oppGoalDownVec.setByPoints(worldModel->getBody().getPos(),
				Point(52.5, -10));

		if (isBetween(oppGoalDownVec.getDirection(),
				oppGoalUpVec.getDirection(),
				worldModel->getViewPartDir(viewPartNum)))
			weight *= haveOppGoalRate;
	}

	// HaveBall weight
	deltaAngle = fabs(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;
}

TurnNeckMode &HeadDecision::getModeRef() const
{
	return (TurnNeckMode &)mode;
}

// Setting functions
void HeadDecision::setMode(TurnNeckMode modeArg)
{
	mode = modeArg;
}