sayencoding.cpp

mersad源码 03年robocup 季军 可以研究一下大家

/*
 *  Copyright 2002-2004, Mersad Team, Allame 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.
 *
 *  Created by: Ahmad Boorghany
 *  Released on Friday 1 April 2005 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>

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

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) * 4;
	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 /= 4;
}

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);
}