/* *  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 5PossiblePoints 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);