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

#include <cmath>
#include <BasicKick.h>
#include <Body.h>
#include <Ball.h>
#include <Degree.h>
#include <Logger.h>

using namespace std;

// class KickToDirectionVelocity

KickToDirectionVelocity::KickToDirectionVelocity(ActionType creator, float direction,
		float velocity, const Ball &ball, const Body &body, float possibleArea): BasicKick(creator),
		direction(direction), velocity(velocity), ball(ball),
		body(body), possibleArea(possibleArea)
{
	float ballStopLength;
	
	kickVector.setAsPolar(velocity, direction);

	trajectoryVector = kickVector;
	
	kickVector = kickVector - ball.getVel();

	kickVector.rotate(-body.getBodyDir());
	ballStopLength = (kickVector.getMagnitude()) / (1 - ball.getDecay());

	kickVector.setAsPolar(needKick(kickVector.getMagnitude(), body , ball),
			kickVector.getDirection());

	kickVector.setAsPolar(kickVector.getMagnitude() /
			ball.getKickPowerRate(), kickVector.getDirection());

	if (kickVector.getMagnitude() > (100 + possibleArea))
		possibleFlag = false;
	else
		possibleFlag = true;
		
	trajectoryVector.setAsPolar(ballStopLength, trajectoryVector.getDirection());

	kickCommand = new KickCommand(creator, kickVector.getMagnitude(),
			Degree::normalizeAngle(kickVector.getDirection()));
}

Command *KickToDirectionVelocity::getCommand()
{
	return kickCommand;
}


// class KickToPointVelocity

KickToPointVelocity::KickToPointVelocity(ActionType creator, const Point &point, float velocity, 
		const Ball &ball, const Body &body, float possibleArea): BasicKick(creator), 
		 point(point), velocity(velocity), ball(ball), body(body), 
		possibleArea(possibleArea)
{
	Vector meToPoint;
	
	meToPoint.setByPoints(ball.getPos(), point);

	kickCommand = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			velocity, ball, body, possibleArea).getCommand();
	possibleFlag = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			velocity, ball, body, possibleArea).isPossible();
	trajectoryVector = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			velocity, ball, body, possibleArea).getTrajVec();

}

Command *KickToPointVelocity::getCommand()
{
	return kickCommand;
}


//	class KickToPointArriveVelocity

KickToPointArriveVelocity::KickToPointArriveVelocity(ActionType creator, const Point &point, 
		float velocity, const Ball &ball, const Body &body, 
		float possibleArea): BasicKick(creator),  point(point), velocity(velocity), 
		ball(ball), body(body), possibleArea(possibleArea)
{
	Vector meToPoint;
	float firstVelocity;
	
	meToPoint.setByPoints(ball.getPos(),point);
	firstVelocity = ball.getDecay() * (velocity - (meToPoint.getMagnitude() * 
			  (1 - (1 / ball.getDecay()))));
	meToPoint.setAsPolar(firstVelocity, meToPoint.getDirection());
	
	kickCommand = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			firstVelocity, ball, body, possibleArea).getCommand();
	possibleFlag = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			firstVelocity, ball, body, possibleArea).isPossible();
	trajectoryVector = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			firstVelocity, ball, body, possibleArea).getTrajVec();

}

Command *KickToPointArriveVelocity::getCommand()
{
	return kickCommand;
}


// class KickToPointCycles

KickToPointCycles::KickToPointCycles(ActionType creator, const Point &point, float cycles, 
		const Ball &ball, const Body &body, float possibleArea): BasicKick(creator), 
		 point(point), cycles(cycles), ball(ball), body(body),
		possibleArea(possibleArea)
{
	Vector meToPoint;
	float firstVelocity;
	
	meToPoint.setByPoints(ball.getPos(),point);
	firstVelocity = (meToPoint.getMagnitude() * (ball.getDecay() - 1)) / 
					(pow(ball.getDecay(), cycles) - 1);
	meToPoint.setAsPolar(firstVelocity, meToPoint.getDirection());
	
	kickCommand = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			firstVelocity, ball, body, possibleArea).getCommand();
	possibleFlag = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			firstVelocity, ball, body, possibleArea).isPossible();
	trajectoryVector = KickToDirectionVelocity(creator, meToPoint.getDirection(),
			firstVelocity, ball, body, possibleArea).getTrajVec();

}

Command *KickToPointCycles::getCommand()
{
	return kickCommand;
}


// class FastKickToDir

FastKickToDir::FastKickToDir(ActionType creator, float dir, const Ball &ball, const Body &body): BasicKick(creator),
		dir(dir), ball(ball), body(body)
{
	float rate = 1 - 0.25 * (ball.getBodyVec().getMagnitude() - body.getSize() -
				ball.getSize()) / body.getKickableMargin() -
				0.25 * fabs(ball.getBodyVec().getDirection() / 180);

	float aLen = body.getMaxPower() * ball.getKickPowerRate() * rate;

	Vector v = ball.getVel();
	v.rotate(-dir);
	float aY = -v.getY();

	float aX;
	if (fabs(aY) > aLen)
	{
		possibleFlag = false;
		aY = aLen * (v.getY() >= 0 ? 1 : -1);
		aX = 0;
	}
	else
	{
		possibleFlag = true;
		aX = sqrt(aLen * aLen - aY * aY);
	}

	Vector a(Point(aX, aY));
	a.rotate(dir);

	float kickAngle = Degree::normalizeAngle(
			a.getDirection() - body.getBodyDir());
	kickCommand = new KickCommand(creator, body.getMaxPower(), kickAngle);
}

Command *FastKickToDir::getCommand()
{
	return kickCommand;
}


// class FastKickToPoint

FastKickToPoint::FastKickToPoint(ActionType creator, Point &point,
		const Ball &ball, const Body &body): BasicKick(creator),
		point(point), ball(ball), body(body)
{
	Vector dirVec;
	dirVec.setByPoints(body.getPos(), point);
	float dir = dirVec.getDirection();

	float rate = 1 - 0.25 * (ball.getBodyVec().getMagnitude() - body.getSize() -
				ball.getSize()) / body.getKickableMargin() -
				0.25 * fabs(ball.getBodyVec().getDirection() / 180);

	float aLen = body.getMaxPower() * ball.getKickPowerRate() * rate;

	Vector v = ball.getVel();
	v.rotate(-dir);
	float aY = -v.getY();

	float aX;
	if (fabs(aY) > aLen)
	{
		possibleFlag = false;
		aY = aLen * (v.getY() >= 0 ? 1 : -1);
		aX = 0;
	}
	else
	{
		possibleFlag = true;
		aX = sqrt(aLen * aLen - aY * aY);
	}

	Vector a(Point(aX, aY));
	a.rotate(dir);

	float kickAngle = Degree::normalizeAngle(
			a.getDirection() - body.getBodyDir());
	kickCommand = new KickCommand(creator, body.getMaxPower(), kickAngle);
}

Command *FastKickToPoint::getCommand()
{
	return kickCommand;
}


// class StopBall

StopBall::StopBall(ActionType creator, const Ball &ball, const Body &body, float possibleArea): BasicKick(creator),
		 ball(ball), body(body), possibleArea(possibleArea)
{
	KickToDirectionVelocity *kickForStopBall;
	kickForStopBall = new KickToDirectionVelocity(creator, ball.getVel().getDirection(), 
			ball.getVel().getMagnitude(),ball, body, possibleArea);

	kickCommand = kickForStopBall->getCommand();
	possibleFlag = kickForStopBall->isPossible();
	trajectoryVector = kickForStopBall->getTrajVec();
	
	delete kickForStopBall;
}

Command *StopBall::getCommand()
{
	return kickCommand;
}


// class BasicKick

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

Vector BasicKick::getKickVec(void)
{
	return kickVector;
}

Vector BasicKick::getTrajVec(void)
{
	return trajectoryVector;
}

bool BasicKick::isPossible(void)
{
	return possibleFlag;
}

float BasicKick::needKick(float velocity, const Body &body, const Ball &ball)
{
	float rate;
	rate = (1 - 0.25 * (ball.getBodyVec().getMagnitude() - body.getSize() -
				ball.getSize()) / body.getKickableMargin() -
				0.25 * (fabs(ball.getBodyVec().getDirection()) / 180));
    return velocity / rate;
}

float BasicKick::directionOfMaxPowerKick(float direction, const Body &body,
	const Ball &ball, float maxKickPower)
{
	float maxEffectivePower;
	float rate;
	float alpha;
	
	rate = (1 - 0.25 * (ball.getBodyVec().getMagnitude() - body.getSize() -
				ball.getSize()) / body.getKickableMargin() -
				0.25 * (fabs(ball.getBodyVec().getDirection()) / 180));

	maxEffectivePower = maxKickPower * ball.getKickPowerRate() * rate;

	alpha = Degree::arcSin(((Degree::sin(direction) * ball.getVel().getX()) - 
						    (Degree::cos(direction) * ball.getVel().getY())) /
						   maxEffectivePower) + direction;
	alpha = Degree::normalizeAngle(alpha);
	return alpha;
}