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

#include <cmath>
#include <assert.h>

#include <BasicTurn.h>
#include <Degree.h>
#include <Player.h>
#include <Body.h>
#include <Ball.h>


BasicTurn::BasicTurn(ActionType creator): BasicAction(creator)
{
}

float BasicTurn::getDeltaAngle()
{
	return deltaAngle;
}
	
float BasicTurn::getNecessaryTurnAngle()
{
	return necessaryTurnAngle;
}

float BasicTurn::getRequestedTargetAngle()
{
	return requestedTargetAngle;
}

float BasicTurn::getPerformedTurnAngle()
{
	return performedTurnAngle;
}

float BasicTurn::getPerformedTurnMoment()
{
	return performedTurnMoment;
}

bool BasicTurn::isOneTurnPossible()
{
	return oneTurnPossible;
}

float BasicTurn::getTurnMoment(float angle, const Player &player)
{
	float moment = 0;
	
	angle = Degree::normalizeAngle(angle);
	moment = angle * (1 + player.getVel().getMagnitude() *
			 player.getInertiaMoment());
	return moment;
}

float BasicTurn::getTurnAngle(float moment, const Player &player)
{
	float angle = 0;

	assert(moment <= player.getMaxMoment() &&
			moment >= player.getMinMoment());
	angle = moment / (1 + player.getVel().getMagnitude() *
			player.getInertiaMoment());
	return angle;
}

int BasicTurn::actionsToTurnAngle(float angle, const Player &player,
		float maxDeltaAngle)
{
	float turnedAngle = 0;
	Player simPlayer(player);
	int i;
	
	angle = fabs(angle) - maxDeltaAngle;
	
	for (i=0;i<=10;i++)
	{
		if (turnedAngle >= angle)
			return i;
		turnedAngle += getTurnAngle(player.getMaxMoment(), player);
		simPlayer.setVel() *= simPlayer.getDecay();
	}
	assert(i < 10);
	return 10;
}


// TurnAngle class

TurnAngle::TurnAngle(ActionType creator, float angle, const Body &body, float maxDeltaAngle)
		: BasicTurn(creator)
{
	float requestedTurnMoment;
	
	necessaryTurnAngle = angle;
	requestedTargetAngle = body.getBodyDir() + angle;
	requestedTurnMoment = getTurnMoment(angle, body);

	if (fabs(requestedTurnMoment) <= body.getMaxMoment() &&
		fabs(requestedTurnMoment) >= body.getMinMoment()) // possible with one turn
	{
		performedTurnMoment = requestedTurnMoment;
		performedTurnAngle = angle;
		oneTurnPossible = true;
		deltaAngle = 0;
	}
	else // not possible with one turn
	{
		if (requestedTurnMoment < 0)
			performedTurnMoment = body.getMinMoment();
		else
			performedTurnMoment = body.getMaxMoment();

		performedTurnAngle = getTurnAngle(performedTurnMoment, body);

		if (fabs(performedTurnAngle - angle) <= maxDeltaAngle) // but it is good
			oneTurnPossible = true;
		else // so it is not good
			oneTurnPossible = false;
		deltaAngle = fabs(performedTurnAngle - angle);
	}
	
	turnCommand = new TurnCommand(creator, performedTurnMoment);
}

Command *TurnAngle::getCommand()
{
	return turnCommand;
}


TurnToAngle::TurnToAngle(ActionType creator, float angle, const Body &body, float maxDeltaAngle)
		: BasicTurn(creator)
{
	TurnAngle *doTurnAngle = NULL;
	
	doTurnAngle = new TurnAngle(creator, angle - body.getBodyDir(), body, maxDeltaAngle);

	turnCommand = doTurnAngle->getCommand();
	deltaAngle = doTurnAngle->getDeltaAngle();
	necessaryTurnAngle = doTurnAngle->getNecessaryTurnAngle();
	requestedTargetAngle = doTurnAngle->getRequestedTargetAngle();
	performedTurnAngle = doTurnAngle->getPerformedTurnAngle();
	performedTurnMoment = doTurnAngle->getPerformedTurnMoment();
	oneTurnPossible = doTurnAngle->isOneTurnPossible();

	delete doTurnAngle;
}

Command *TurnToAngle::getCommand()
{
	return turnCommand;
}


TurnToPoint::TurnToPoint(ActionType creator, const Point &point, const Body &body, float maxDeltaAngle)
		: BasicTurn(creator)
{
	Vector newBodyPosition;
	Vector directionVector;
	TurnToAngle *doTurnToAngle = NULL;
	
	newBodyPosition = (body.getPos()) + (body.getVel());

	directionVector.setByPoints(newBodyPosition,point);
	
	doTurnToAngle = new TurnToAngle(creator, directionVector.getDirection(), body, maxDeltaAngle);

	turnCommand = doTurnToAngle->getCommand();
	deltaAngle = doTurnToAngle->getDeltaAngle();
	necessaryTurnAngle = doTurnToAngle->getNecessaryTurnAngle();
	requestedTargetAngle = doTurnToAngle->getRequestedTargetAngle();
	performedTurnAngle = doTurnToAngle->getPerformedTurnAngle();
	performedTurnMoment = doTurnToAngle->getPerformedTurnMoment();
	oneTurnPossible = doTurnToAngle->isOneTurnPossible();

	delete doTurnToAngle;
}

Command *TurnToPoint::getCommand()
{
	return turnCommand;
}


TurnToObject::TurnToObject(ActionType creator, const Object &object, const Body &body, float maxDeltaAngle)
		: BasicTurn(creator)
{
	TurnToPoint *doTurnToPoint = NULL;
	
	doTurnToPoint = new TurnToPoint(creator, object.getPos(), body, maxDeltaAngle);

	turnCommand = doTurnToPoint->getCommand();
	deltaAngle = doTurnToPoint->getDeltaAngle();
	necessaryTurnAngle = doTurnToPoint->getNecessaryTurnAngle();
	requestedTargetAngle = doTurnToPoint->getRequestedTargetAngle();
	performedTurnAngle = doTurnToPoint->getPerformedTurnAngle();
	performedTurnMoment = doTurnToPoint->getPerformedTurnMoment();
	oneTurnPossible = doTurnToPoint->isOneTurnPossible();

	delete doTurnToPoint;
}

Command *TurnToObject::getCommand()
{
	return turnCommand;
}