body.cpp

2006年世界杯足球赛2D仿真组第16名的源代码。在此代码上随便改改

/*
 *  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 <assert.h>

#include <Body.h>
#include <Degree.h>
#include <Config.h>
#include <Logger.h>
#include <Command.h>
#include <SExpression.h>
#include <PossiblePoints.h>

using namespace std;
using namespace Degree;

Body::Body()
{
	collisionStatus = TA_DONT_KHOW;
}

void Body::parseSenseBody(const SExpression &exp,Side fieldSide)
{
	SExpAtomic *at;
	SExpression *se1,*se2;

	at = dynamic_cast<SExpAtomic *>(exp[0]);
	assert(at);
	assert(at->asString() == "sense_body");

	// preparing.
	lastKickCount = kickCount;
	lastDashCount = dashCount;
	lastTurnCount = turnCount;
	lastSayCount = sayCount;
	lastTurnNeckCount = turnNeckCount;
	lastCatchCount = catchCount;
	lastMoveCount = moveCount;
	lastChangeViewCount = changeViewCount;
	lastArmExpires = armExpires;
	lastArmCount = armCount;
	lastFocusCount = focusCount;
	lastTackleExpires = tackleExpires;
	lastTackleCount = tackleCount;

	// SenseBody cycle
	at = dynamic_cast<SExpAtomic *>(exp[1]);
	assert(at);
	senseBodyCycle = at->asInt();

	// View mode
	se1 = dynamic_cast<SExpression *>(exp[2]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "view_mode");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	if (at->asString()[0] == 'l')
		vmQuality = VMQ_LOW;
	else
	{
		assert(at->asString()[0] == 'h');
		vmQuality = VMQ_HIGH;
	}

	at = dynamic_cast<SExpAtomic *>((*se1)[2]);
	assert(at);
	if (at->asString()[0] == 'w')
		vmWidth = VMW_WIDE;
	else
	{
		assert(at->asString()[0] == 'n');

		if (at->asString()[1] == 'a')
			vmWidth = VMW_NARROW;
		else
		{
			assert(at->asString()[1] == 'o');
			vmWidth = VMW_NORMAL;
		}
	}

	// Stamina
	se1 = dynamic_cast<SExpression *>(exp[3]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "stamina");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	stamina = at->asFloat();

	at = dynamic_cast<SExpAtomic *>((*se1)[2]);
	assert(at);
	effort = at->asFloat();

	// Speed
	se1 = dynamic_cast<SExpression *>(exp[4]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "speed");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	speedAmount = at->asFloat();

	at = dynamic_cast<SExpAtomic *>((*se1)[2]);
	assert(at);
	speedDir = at->asFloat();

	// Head angle
	se1 = dynamic_cast<SExpression *>(exp[5]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "head_angle");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	relativeHeadDir = at->asFloat();

	// Kick
	se1 = dynamic_cast<SExpression *>(exp[6]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "kick");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	kickCount = at->asInt();

	// Dash
	se1 = dynamic_cast<SExpression *>(exp[7]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "dash");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	dashCount = at->asInt();

	// Turn
	se1 = dynamic_cast<SExpression *>(exp[8]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "turn");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	turnCount = at->asInt();

	// Say
	se1 = dynamic_cast<SExpression *>(exp[9]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "say");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	sayCount = at->asInt();

	// Turn Neck
	se1 = dynamic_cast<SExpression *>(exp[10]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "turn_neck");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	turnNeckCount = at->asInt();

	// Catch
	se1 = dynamic_cast<SExpression *>(exp[11]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "catch");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	catchCount = at->asInt();

	// Move
	se1 = dynamic_cast<SExpression *>(exp[12]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "move");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	moveCount = at->asInt();

	// Change View
	se1 = dynamic_cast<SExpression *>(exp[13]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "change_view");
	
	at = dynamic_cast<SExpAtomic *>((*se1)[1]);
	assert(at);
	changeViewCount = at->asInt();

	// Arm
	se1 = dynamic_cast<SExpression *>(exp[14]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "arm");
	
	se2 = dynamic_cast<SExpression *>((*se1)[1]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "movable");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	armMovable = at->asInt();

	se2 = dynamic_cast<SExpression *>((*se1)[2]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "expires");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	armExpires = at->asInt();

	se2 = dynamic_cast<SExpression *>((*se1)[3]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "target");

	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	armTargetDist = at->asFloat();

	at = dynamic_cast<SExpAtomic *>((*se2)[2]);
	assert(at);
	armTargetDir = at->asFloat();

	se2 = dynamic_cast<SExpression *>((*se1)[4]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "count");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	armCount = at->asInt();

	// Focus
	se1 = dynamic_cast<SExpression *>(exp[15]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "focus");
	
	se2 = dynamic_cast<SExpression *>((*se1)[1]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "target");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	if (at->asString() == "none")
		focusTargetUniNum = 0; // it means there is no focus.
	else
	{
		if (at->asString()[0] == 'l')
			if (fieldSide == SI_LEFT)
				focusTargetTeamId = TID_TEAMMATE;
			else
				focusTargetTeamId = TID_OPPONENT;
		else
		{
			assert(at->asString()[0] == 'r');
			if (fieldSide == SI_RIGHT)
				focusTargetTeamId = TID_TEAMMATE;
			else
				focusTargetTeamId = TID_OPPONENT;
		}

		at = dynamic_cast<SExpAtomic *>((*se2)[2]);
		assert(at);
		focusTargetUniNum = at->asInt();
	}

	se2 = dynamic_cast<SExpression *>((*se1)[2]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "count");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	focusCount = at->asInt();

	// Tackle
	se1 = dynamic_cast<SExpression *>(exp[16]);
	assert(se1);
	at = dynamic_cast<SExpAtomic *>((*se1)[0]);
	assert(at);
	assert(at->asString() == "tackle");
	
	se2 = dynamic_cast<SExpression *>((*se1)[1]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "expires");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	tackleExpires = at->asInt();

	se2 = dynamic_cast<SExpression *>((*se1)[2]);
	assert(se2);
	at = dynamic_cast<SExpAtomic *>((*se2)[0]);
	assert(at);
	assert(at->asString() == "count");
	at = dynamic_cast<SExpAtomic *>((*se2)[1]);
	assert(at);
	tackleCount = at->asInt();

	// completing.
//	posDeviation = 0;
//	validPosDeviation = 0;
//	velDeviation = 0;

	relativeHeadDir *= -1;
	speedDir *= -1;
	armTargetDir *= -1;

	headDir = normalizeAngle(bodyDir + relativeHeadDir);

	if (vmWidth == VMW_NARROW)
		viewAngle = 22.5;
	else if (vmWidth == VMW_NORMAL)
		viewAngle = 45;
	else if (vmWidth == VMW_WIDE)
		viewAngle = 90;
}

void Body::update(FieldFlag **flags, FieldGoal **goals, FieldLine **lines,
		unsigned flagsNum, unsigned goalsNum, unsigned linesNum)
{
	float angle;
	unsigned i;
	unsigned farestLine = 0;

	// Updating headDir and bodyDir
	if (linesNum != 0)
	{
		for (i = 1; i < linesNum; i++)
			if (lines[i]->getHeadVec().getMagnitude() >
				lines[farestLine]->getHeadVec().getMagnitude())
				farestLine = i;

		angle = lines[farestLine]->getHeadVec().getDirection();
		headDir = lines[farestLine]->getPrependOrient() -
				((angle < 0) ? (90 + angle) : -(90 - angle));

		headDir = normalizeAngle(headDir);
		bodyDir = normalizeAngle(headDir - relativeHeadDir);
	}	

	// Updating Position
	if (flagsNum > 0)
		position = possiblePoints.calculateBodyPosition(*this, (const FieldFlag**)(flags), flagsNum, posDeviation, quantizeStep);

	validPosDeviation = posDeviation;

	// Updating Velocity
	velocity.setAsPolar(speedAmount,headDir + speedDir);
}

void Body::simulateByDynamics(bool realBody)
{
	position += velocity;
	velocity *= decay;