/* *  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 functionsvoid HeadDecisionForm::reset(){	tnMode = TNM_LOOK_NORMALLY;}// HeadDecision's functionsHeadDecision::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;}