sayencoding.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: Ahmad Boorghany
 *  and is modified by: Sassan Haradji
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>
#include <Logger.h>

#include <Basics.h>
#include <Degree.h>
#include <SayEncoding.h>
#include <cassert>

using namespace std;
using namespace Basics;
using namespace Degree;

string SayEncoding::sayCodeKey = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ-().+*/?<>_";

unsigned SayEncoding::getCharNum(char ch)
{
	return sayCodeKey.find(ch);
}

void SayEncoding::encodeObjectPosition(float x, float y, string &code)
{
	int i;
	int codeNum = 0;
	int tempCodeLen = 0;
	string tempCodeStr;

	if (x > 53)
		x = 52.8;
	if (x < -53)
		x = -53;
	if (y > 35)
		y = 34.8;
	if (y < -35)
		y = -35;

	x += 53;
	x *= 5;
	y += 35;
	y *= 5;

	x = floor(x);
	y = floor(y);

	codeNum = (int)y * 530 + (int)x;
	tempCodeLen = changeIntBase(codeNum, sayCodeKey.length(), tempCodeStr);

	for (i = 0; i < 3; i++)
		if (i < tempCodeLen)
			code += sayCodeKey[tempCodeStr[i]];
		else
			code += sayCodeKey[0];
}

void SayEncoding::decodeObjectPosition(const string &code, unsigned codePtr,
		float &x, float &y)
{
	int codeNum;

	codeNum = sayCodeKey.find(code[codePtr + 0]) * 1 +
			sayCodeKey.find(code[codePtr + 1]) * 73 +
			sayCodeKey.find(code[codePtr + 2]) * 73 * 73;

	x = codeNum % 530;
	y = (int)(codeNum / 530.00);

	x /= 5;
	x -= 53;

	y /= 5;
	y -= 35;
}

void SayEncoding::encodeObjectVelocity(float magnitude, float direction, 
		string &code)
{
	int i;
	int codeNum = 0;
	int tempCodeLen = 0;
	string tempCodeStr;

	if (magnitude > 2.7)
		magnitude = 2.7;

	magnitude *= 100;
	magnitude = floor(magnitude);
	direction = absoluteAngle(direction) * 3.5;
	direction = floor(direction);

	codeNum = (int)magnitude * 1440 + (int)direction;
	tempCodeLen = changeIntBase(codeNum, sayCodeKey.length(), tempCodeStr);
	for (i = 0; i < 3; i++)
		if (i < tempCodeLen)
			code += sayCodeKey[tempCodeStr[i]];
		else
			code += sayCodeKey[0];
}

void SayEncoding::decodeObjectVelocity(const string &code, unsigned codePtr,
		float &magnitude, float &direction)
{
	int codeNum = sayCodeKey.find(code[codePtr + 0]) * 1 +
			sayCodeKey.find(code[codePtr + 1]) * 73 +
			sayCodeKey.find(code[codePtr + 2]) * 73 * 73;

	direction = codeNum % 1440;
	magnitude = floor(codeNum / 1440.00);

	magnitude /= 100;
	direction /= 3.5;
}

void SayEncoding::encodeWeight(float weight, float start,
		float end, std::string &code)
{
	unsigned codeNum = (unsigned)Basics::reRate(weight, start, end,
			0, 73 * 73 * 73 - 1);

	string tempCodeStr;
	unsigned tempCodeLen = changeIntBase(codeNum,
			sayCodeKey.length(), tempCodeStr);
	for (unsigned i = 0; i < 3; i++)
		if (i < tempCodeLen)
			code += sayCodeKey[tempCodeStr[i]];
		else
			code += sayCodeKey[0];
}

void SayEncoding::decodeWeight(const std::string &code,
		unsigned codePtr, float &weight, float start, float end)
{
	int codeNum = sayCodeKey.find(code[codePtr + 0]) * 1 +
			sayCodeKey.find(code[codePtr + 1]) * 73 +
			sayCodeKey.find(code[codePtr + 2]) * 73 * 73;

	weight = Basics::reRate(codeNum, 0, 73 * 73 * 73 - 1,
			start, end);
}

void SayEncoding::encodeWMSend(vector<Vector> positions, string &code)
{
	if (positions.size() != 5)
	{
		LOG << "ERROR : THIS FUNCTION ONLY IS FOR 5 POSITIONS"
					<< endl;
		assert(0);
	}

	float mag = positions[0].getMagnitude();
	float dir = positions[0].getDirection();

	mag = floor(mag * 4);
	dir = floor(absoluteAngle(dir) - 45);
	long long unsigned posCode = int(mag) * 270 + int(dir);

	for (unsigned i = 1; i < 5; i++)
	{
		float mag = positions[i].getMagnitude();
		float dir = positions[i].getDirection();

		mag = floor(mag * 4);
		dir = floor(dir / 2);

		unsigned codeToAdd = int(mag) * (28 / 2) + int(dir);

		posCode *= (20 * 4) * (28 / 2);
		posCode += codeToAdd;
	}
	string tempCodeStr;
	unsigned tempCodeLen = 0;

	for (tempCodeLen = 0; posCode > 72; tempCodeLen++)
	{
		tempCodeStr += (char)(posCode % 72);
		posCode -= posCode % 72;
		posCode = (long long unsigned)(posCode / 72);
	}
	tempCodeStr += (char)posCode;
	tempCodeLen++;
	
	for (unsigned i = 0; i < 9; i++)
		if (i < tempCodeLen)
			code += sayCodeKey[tempCodeStr[i]];
		else
			code += sayCodeKey[0];
}

void SayEncoding::decodeWMSend(const string &code, vector<Vector> &positions)
{
	long long unsigned posCode = 0;

	for (unsigned i = 0; i < 9; i++)
		posCode += sayCodeKey.find(code[i]) *
				(long long unsigned)(pow((long double)72, int(i)));

	positions.clear();

	for (unsigned i = 0; i < 5; i++)
		positions.push_back(Vector(Point(0, 0)));

	for (unsigned i = 1; i < 5; i++)
	{
		unsigned tmpCode = (posCode % ((20 * 4) * (28 / 2)));
		float mag = unsigned(tmpCode / (28.00 / 2));
		float dir = tmpCode % (28 / 2);
		mag /= 4;
		dir *= 2;

		Vector tmpPos;
		tmpPos.setAsPolar(mag, dir);

		positions[5 - i] = tmpPos;

		posCode -= tmpCode;
		posCode /= (20 * 4) * (28 / 2);
	}

	unsigned tmpCode = posCode;
	float mag = unsigned(tmpCode / 270.00);
	float dir = tmpCode % (270);
	mag /= 4.00;
	dir += 45;

	Vector tmpPos;
	tmpPos.setAsPolar(mag, dir);
	positions[0] = tmpPos;
}

void SayEncoding::encodeRadarObject(unsigned id, float magnitude,
		float direction, std::string &code)
{
	int codeNum = 0;
	int tempCodeLen = 0;
	string tempCodeStr;

	if (id > 21)
		assert(0);

	if (magnitude > 29.5)
		magnitude = 29.5;
	direction = Degree::absoluteAngle(direction);

//LOG << "\t\tmagnitude: " << magnitude << endl;
//LOG << "\t\tdirection: " << direction << endl;

	magnitude *= 2;
	direction /= 2;

	magnitude = floor(magnitude);
	direction = floor(direction);

	codeNum = (int)id * 180*60 + (int)magnitude * 180 + (int)direction;
	tempCodeLen = changeIntBase(codeNum, sayCodeKey.length(), tempCodeStr);

//LOG << "\t\tid: " << id << endl;
//LOG << "\t\tcodeNum: " << codeNum << endl;

	for (int i = 0; i < 3; i++)
		if (i < tempCodeLen)
			code += sayCodeKey[tempCodeStr[i]];
		else
			code += sayCodeKey[0];
}

void SayEncoding::decodeRadarObject(const std::string &code,
		unsigned codePtr, unsigned &id, float &magnitude, float &direction)
{
	int codeNum;

//LOG << "XYZ: " << code[codePtr + 0] << code[codePtr + 1] << code[codePtr + 2] << endl;

	codeNum = sayCodeKey.find(code[codePtr + 0]) * 1 +
			sayCodeKey.find(code[codePtr + 1]) * 73 +
			sayCodeKey.find(code[codePtr + 2]) * 73 * 73;

	id = (unsigned)((float)codeNum / (180*60));
	int temp = codeNum % (180*60);
	magnitude = (int)(temp / 180);
	direction = temp % 180;

	magnitude /= 2;
	direction *= 2;

//LOG << "\t\tmagnitude: " << magnitude << endl;
//LOG << "\t\tdirection: " << direction << endl;
//LOG << "\t\tid: " << id << endl;
//LOG << "\t\tcodeNum: " << codeNum << endl;

}