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

#include <PossiblePoints.h>
#include <Defines.h>
#include <Degree.h>
#include <Config.h>

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

#define POINTS_TABLE_PPDX 0.20f
#define POINTS_TABLE_PPDY 0.20f

#define POINTS_TABLE_BPDX 0.18f
#define POINTS_TABLE_BPDY 0.18f

#define POINTS_TABLE_VDX 0.06f
#define POINTS_TABLE_VDY 0.06f

#define SQRT2 M_SQRT2

#define GOOD_TABLE_POINTS 4

#define VELOCITY_SAFE_AREA 0.14f

#define VALID_POS_DEVIATION 5

PossiblePoints possiblePoints;

PossiblePoints::PossiblePoints()
{
	data = NULL;
	pointsTablePW = 0;
	pointsTablePH = 0;
}

PossiblePoints::~PossiblePoints()
{
	destroy();
}

void PossiblePoints::view()
{
	/*
		int i, j;
	// MUST USE NEW STRING & LOGMAKER
		string g;
		g.setlength(pointsTablePW);
		for (i = pointsTablePH-1;i>=0;i--)
		{
			for (j=0;j < pointsTablePW;j++)
				g[j] *= char(data[j+pointsTablePW * i]+65);
				
			LogMaker.Log(LV_WModel, g);
		}
		LogMaker.Log(LV_WModel);
	*/
}


void PossiblePoints::initTableByMiddle(float middleX, float middleY,
	float width, float heigth)
{
	destroy();

	left = middleX - (width / 2);	
	down = middleY - (heigth / 2);	
	pointsTablePW = (int) (floor((width / pointsTablePDX) + 1));
	pointsTablePH = (int) (floor((heigth / pointsTablePDY) + 1));
	dX = width / pointsTablePW;
	dY = heigth / pointsTablePH;
	xAverage = middleX;
	yAverage = middleY;

	//LogMaker.Log(LV_WModel,"left, down= ",left," ",down);
	//LogMaker.Log(LV_WModel,"PTPW, PTPH= ",pointsTablePW," ",pointsTablePH);
	//LogMaker.Log(LV_WModel,"DX, DY= ",dX," ",dY);
	//LogMaker.Log(LV_WModel,"middleX, middleY= ",middleX," ",middleY);

	data = new int [pointsTablePW * pointsTablePH];

	reset();
}


void PossiblePoints::initTableByNorthEast(float leftArg, float downArg,
	float width, float heigth)
{
	destroy();

	left = leftArg;
	down = downArg;
	pointsTablePW = (int) (floor((width / pointsTablePDX) + 1));
	pointsTablePH = (int) (floor((heigth / pointsTablePDY) + 1));
	dX = width / pointsTablePW;
	dY = heigth / pointsTablePH;
	xAverage = (left + (width / 2));
	yAverage = (down + (heigth / 2));

	//LogMaker.Log(LV_WModel,"left, down= ",left," ",down);
	//LogMaker.Log(LV_WModel,"PTPW, PTPH= ",pointsTablePW," ",pointsTablePH);
	//LogMaker.Log(LV_WModel,"DX, DY= ",dX," ",dY);
	//LogMaker.Log(LV_WModel,"width, heigth= ",width," ",heigth);

	data = new int [pointsTablePW * pointsTablePH];

	reset();
}


void PossiblePoints::initTableForVelocityByNorthEast(float leftArg,
	float downArg, float width, float heigth)
{
	destroy();

	left = leftArg;
	down = downArg;
	pointsTablePW = (int) (floor((width / POINTS_TABLE_VDX) + 1));
	pointsTablePH = (int) (floor((heigth / POINTS_TABLE_VDY) + 1));
	dX = width / pointsTablePW;
	dY = heigth / pointsTablePH;
	xAverage = (left + (width / 2));
	yAverage = (down + (heigth / 2));

	//LogMaker.Log(LV_WModel,"left, down= ",left," ",down);
	//LogMaker.Log(LV_WModel,"PTPW, PTPH= ",pointsTablePW," ",pointsTablePH);
	//LogMaker.Log(LV_WModel,"DX, DY= ",dX," ",dY);
	//LogMaker.Log(LV_WModel,"width, heigth= ",width," ",heigth);

	data = new int [pointsTablePW * pointsTablePH];

	reset();
}


void PossiblePoints::reset()
{
	//LogMaker.Log(LV_WModel,"PP reset");
	for (int i = 0; i < pointsTablePW; i++)
		for (int j = 0; j < pointsTablePH; j++)
			data[i + pointsTablePW * j] = 0;

	maxData = 0;
}

void PossiblePoints::destroy()
{
	//LogMaker.Log(LV_WModel,"PP destroy");
	if (data && pointsTablePW && pointsTablePH)
	{
		delete[] data;
		data = NULL;
	}
}


void PossiblePoints::setByFlags(const FieldFlag **flags, int nFlags,
	float headAngle, float quantizeStep)
{
	int i;
	float a, b, c, d, e;
	float distMin, distMax, dirMin, dirMax;

	Vector flagPosition;
	Vector ray;

	if (nFlags == 0)
		return;

	//LogMaker.Log(LV_WModel,"PP addFlags Start NF, DMax=",nFlags," ",maxData);

	// Initialize Table With Nearest flag
	const FieldFlag* nearestFlag;
	nearestFlag = flags[0];
	for (i = 0; i < nFlags; i++)
		if (flags[i]->getSeeDistance() < nearestFlag->getSeeDistance())
			nearestFlag = flags[i];

	//	//LogMaker.Log(LV_WModel,"PP Nearest XY=",nearestFlag->getPos()->getX()," ",nearestFlag->getPos()->getY());
	//	//LogMaker.Log(LV_WModel,"PP Nearest DD=",nearestFlag->getSeeDistance()," ",nearestFlag->getSeeDirection());

	a = nearestFlag->getSeeDistance();
	b = log(a);
	c = quantize(b, quantizeStep);
	quantizeRange(c, quantizeStep, d, e);
	distMin = exp(d);
	distMax = exp(e);


	if (distMin > distMax)
	{
		a = distMax;
		distMax = distMin;
		distMin = a;
	}

	//	//LogMaker.Log(LV_WModel,"PP Nearest Dist MM=",distMin," ",distMax);

	a = nearestFlag->getSeeDirection();
	a += headAngle;

	quantizeRange(a, 1, dirMin, dirMax);

	//	//LogMaker.Log(LV_WModel,"PP headAngle=",headAngle);
	//	//LogMaker.Log(LV_WModel,"PP Nearest Dir MM=",dirMin," ",dirMax);

	flagPosition = (nearestFlag->getPos());

	ray.setAsPolar(distMin, dirMin);
	ray = flagPosition - ray;
	a = ray.getX(); // xmin
	b = ray.getY(); // ymin
	c = ray.getX(); // xmax
	d = ray.getY(); // ymax

	//	//LogMaker.Log(LV_WModel,"PP Nearest 1XMin XMax=",a," ",c);
	//	//LogMaker.Log(LV_WModel,"PP Nearest 1YMin YMax=",b," ",d);


	ray.setAsPolar(distMin, dirMax);
	ray = flagPosition - ray;
	if (ray.getX() < a)
		a = ray.getX();
	if (ray.getX() > c)
		c = ray.getX();
	if (ray.getY() < b)
		b = ray.getY();
	if (ray.getY() > d)
		d = ray.getY();

	//	//LogMaker.Log(LV_WModel,"PP Nearest 2XMin XMax=",a," ",c);
	//	//LogMaker.Log(LV_WModel,"PP Nearest 2YMin YMax=",b," ",d);


	ray.setAsPolar(distMax, dirMin);
	ray = flagPosition - ray;
	if (ray.getX() < a)
		a = ray.getX();
	if (ray.getX() > c)
		c = ray.getX();
	if (ray.getY() < b)
		b = ray.getY();
	if (ray.getY() > d)
		d = ray.getY();

	//	//LogMaker.Log(LV_WModel,"PP Nearest 3XMin XMax=",a," ",c);
	//	//LogMaker.Log(LV_WModel,"PP Nearest 3YMin YMax=",b," ",d);


	ray.setAsPolar(distMax, dirMax);
	ray = flagPosition - ray;
	if (ray.getX() < a)
		a = ray.getX();
	if (ray.getX() > c)
		c = ray.getX();
	if (ray.getY() < b)
		b = ray.getY();
	if (ray.getY() > d)
		d = ray.getY();

	//	//LogMaker.Log(LV_WModel,"PP Nearest 4XMin XMax=",a," ",c);
	//	//LogMaker.Log(LV_WModel,"PP Nearest 4YMin YMax=",b," ",d);

	dirMin = Degree::absoluteAngle(dirMin);
	dirMax = Degree::absoluteAngle(dirMax);

	if (Degree::isBetween(dirMin, dirMax, 90))
	{
		ray.setAsPolar(distMax, 90);
		ray = flagPosition - ray;
		//		if (ray.getX() < a) a = ray.getX();
		//	  if (ray.getX() > c) c = ray.getX();
		if (ray.getY() < b)
			b = ray.getY();
		if (ray.getY() > d)
			d = ray.getY();

		//	//LogMaker.Log(LV_WModel,"PP Nearest 5XMin XMax=",a," ",c);
		//	//LogMaker.Log(LV_WModel,"PP Nearest 5YMin YMax=",b," ",d);
	}

	if (Degree::isBetween(dirMin, dirMax, 270))
	{
		ray.setAsPolar(distMax, -90);
		ray = flagPosition - ray;
		//		if (ray.getX() < a) a = ray.getX();
		//	  if (ray.getX() > c) c = ray.getX();
		if (ray.getY() < b)
			b = ray.getY();
		if (ray.getY() > d)
			d = ray.getY();

		//	//LogMaker.Log(LV_WModel,"PP Nearest 6XMin XMax=",a," ",c);
		//	//LogMaker.Log(LV_WModel,"PP Nearest 6YMin YMax=",b," ",d);
	}

	if (Degree::isBetween(dirMin, dirMax, 0))
	{
		ray.setAsPolar(distMax, 0);
		ray = flagPosition - ray;
		if (ray.getX() < a)
			a = ray.getX();
		if (ray.getX() > c)
			c = ray.getX();
		//		if (ray.getY() < b) b = ray.getY();
		//	  if (ray.getY() > d) d = ray.getY();

		//	//LogMaker.Log(LV_WModel,"PP Nearest 7XMin XMax=",a," ",c);
		//	//LogMaker.Log(LV_WModel,"PP Nearest 7YMin YMax=",b," ",d);
	}

	if (Degree::isBetween(dirMin, dirMax, 180))
	{
		ray.setAsPolar(distMax, 180);
		ray = flagPosition - ray;
		if (ray.getX() < a)
			a = ray.getX();
		if (ray.getX() > c)
			c = ray.getX();
		//		if (ray.getY() < b) b = ray.getY();
		//		if (ray.getY() > d) d = ray.getY();

		//	//LogMaker.Log(LV_WModel,"PP Nearest 8XMin XMax=",a," ",c);
		//	//LogMaker.Log(LV_WModel,"PP Nearest 8YMin YMax=",b," ",d);
	}

	initTableByNorthEast(a, b, c - a, d - b);

	//	//LogMaker.Log(LV_WModel,"PP Table Inited");
	//	view();
	// for each flag check points

	int n = 0;
	int t = 0;

	//	i = (int)(pointsTablePW * pointsTablePH/10);

	// temp
	int tempArray1[60];
	float tempArray2[60];
	int tempValue1 = 0;
	float tempValue2 = 0;
	int t2 = 0;

	for (t = 0; t < nFlags; t++)
	{
		tempArray1[t] = t;
		tempArray2[t] = flags[t]->getSeeDistance();
	}

	for (t = 0; t < nFlags; t++)
	{
		for (t2 = t; t2 < nFlags; t2++)
		{
			if (tempArray2[t] > tempArray2[t2])
			{
				tempValue1 = tempArray1[t]; tempArray1[t] = tempArray1[t2]; tempArray1[t2] = tempValue1;
				tempValue2 = tempArray2[t]; tempArray2[t] = tempArray2[t2]; tempArray2[t2] = tempValue2;
			}
		}
	}

	for (t = 0; t < nFlags; t++)
	{
		n = addFlag(flags[tempArray1[t]], headAngle, quantizeStep);
		//LogMaker.Log(LV_WModel,"PP addFlag N, relVelVector=",t," ",n);

		if (n <= GOOD_TABLE_POINTS && n > 0)
			break; // if is is a good table ,don't check the other flags to IMPROVE SPEED
	}

	//LogMaker.Log(LV_WModel,"PP addFlags End N, relVelVector=",t," ",n);