worldmodel.cpp

用robocup2d改编设计的２d球队。

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the University of Amsterdam nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*! \file WorldModel.cpp
<pre>
<b>File:</b>          WorldModel.cpp
<b>Project:</b>       Robocup Soccer Simulation Team: UvA Trilearn
<b>Authors:</b>       Jelle Kok
<b>Created:</b>       12/02/2001
<b>Last Revision:</b> $ID$
<b>Contents:</b>      class definitions of WorldModel. This class contains
               the methods of the WorldModel that process the incoming
               information and analyze the current state of the world.
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
12/02/2001       Jelle Kok       Initial version created
</pre>
*/

#include<stdio.h>    // needed for printf
#include<errno.h>    // needed for ETIMEDOUT
#ifdef WIN32
  #include<windows.h>
#else
  #include<strings.h>  // needed for strcpy
  #include<pthread.h>  // needed for pthread_mutex_init
#endif
#include<string.h>   // needed for strcpy
#include<math.h>     // needed for erf
#include<map>        // needed for map
#include"WorldModel.h"
#define PAI 3.1415926
/*****************************************************************************/
/********************** CLASS WORLDMODEL *************************************/
/*******^**********************************************************************
*/      
/**
        ^
        |
      45|44  
____59__|__30_______>
    0   |  29
      14|15
        | 
*/
bool WorldModel::CanseeAngel(double angel)
{
	double MyAngel=getAgentGlobalNeckAngle();
	int tempAngel=MyAngel-angel;
	tempAngel=tempAngel%360;
	if(fabs(tempAngel)<24.0)return true;
	else return false;
}
bool WorldModel::IsCanKick()
{
        PlayModeT  pm = getPlayMode();
        bool playon=(pm==PM_PLAY_ON);
        return isKickInUs  ( pm ) || isFreeKickUs  ( pm ) || isCornerKickUs ( pm)||playon|| isKickOffUs ( pm ) || isOffsideThem ( pm ) || isFreeKickFaultThem( pm)|| isBackPassThem( pm) ;
}
void WorldModel::CleanStation()
{
	for(int i=0;i<SM;i++)
		for(int j=0;j<SN;j++)
		{
			DecitionWe[i][j]=0;
			DecitionTheir[i][j]=0;
		}
}
void WorldModel::UpStation()
{
	CleanStation();
	VecPosition posAgent = getAgentGlobalPosition();
        VecPosition tempPos;
        double ang,dist;
        int iIndex;
        int m,n;
        for(   ObjectT o=iterateObjectStart(iIndex,OBJECT_SET_TEAMMATES);
                        o!=OBJECT_ILLEGAL;
                        o=iterateObjectNext(iIndex,OBJECT_SET_TEAMMATES))
        {
        	tempPos=getGlobalPosition(o);
                dist=posAgent.getDistanceTo(tempPos);
                ang=(tempPos-posAgent).getDirection();
                if(dist<30.0)
                {
                	m=(ang+180.0)*SM/360.0;
                	n=dist*SN/30.0;
                	DecitionWe[m][n]+=1;	
                }
        }
        iterateObjectDone(iIndex);
        for(   ObjectT o=iterateObjectStart(iIndex,OBJECT_SET_OPPONENTS);
                        o!=OBJECT_ILLEGAL;
                        o=iterateObjectNext(iIndex,OBJECT_SET_OPPONENTS))
        {
        	tempPos=getGlobalPosition(o);
                dist=posAgent.getDistanceTo(tempPos);
                ang=(tempPos-posAgent).getDirection();
                if(dist<30.0)
                {
                	m=(ang+180.0)*SM/360.0;
                	n=dist*SN/30.0;
                	DecitionTheir[m][n]+=1;	
                }
        }
        iterateObjectDone(iIndex);
}
bool WorldModel::HasOpponetIn(int angel1,int angel2,int dist)
{

	bool has=false;
	for(int i=angel1;i<angel2+1;i++)
		for(int j=0;j<dist+1;j++)
	if(DecitionTheir[i][j]>0)has=true;
	return has;
}
int WorldModel::ChangeAngelToInt(double angel,bool Is360)
{
	int mn; 
	if(Is360)
		mn=((angel+180.0)*SM/360.0);
	else mn=((angel+90.0)*HALF_SM/180.0);
}
double    WorldModel::getFrontLine()
{
        VecPosition posOpp;
        int iIndex;
                double dist1=-10;
        double dist2=-10;
        for(   ObjectT o=iterateObjectStart(iIndex,OBJECT_SET_OPPONENTS,0.5);
                        o!=OBJECT_ILLEGAL;
                        o=iterateObjectNext(iIndex,OBJECT_SET_OPPONENTS,0.5))
        {
                if(o!=getOppGoalieType())
                {
                        posOpp=getGlobalPosition(o);
                        if(dist1>posOpp.getX())
                        {
                                dist1=posOpp.getX();
                        }
                        else if(dist2>posOpp.getX())
                        {
                                dist2=posOpp.getX();
                        }
                }
        }
        iterateObjectDone(iIndex);
                return (dist1+dist2)/2;
}
ObjectT WorldModel::getBestObject(double dDist)
{
        ObjectT o1,o2,o3;
        o1=getAgentObjectType();
        VecPosition  posAgent=getAgentGlobalPosition();
        VecPosition  pos(-52.5,-34);
        if(o1==OBJECT_TEAMMATE_1)
        {
                if(posAgent.getY()>0)
                {
                        o2=OBJECT_TEAMMATE_2;
                        o3=OBJECT_TEAMMATE_1;
                }
                else
                {
                        o2=OBJECT_TEAMMATE_5;
                        o3=OBJECT_TEAMMATE_1;
                }
        }
        else if(o1==OBJECT_TEAMMATE_2)
        {
                o2=OBJECT_TEAMMATE_10;
                o3=OBJECT_TEAMMATE_8;
        }
        else if(o1==OBJECT_TEAMMATE_3)
        {
                o2=OBJECT_TEAMMATE_7;
                o3=OBJECT_TEAMMATE_2;

        }
        else if(o1==OBJECT_TEAMMATE_4)
        {
                o2=OBJECT_TEAMMATE_8;
                o3=OBJECT_TEAMMATE_5;
        }
        else if(o1==OBJECT_TEAMMATE_6)
        {
                if(posAgent.getX()>26)
                        o2=OBJECT_TEAMMATE_9;
                else
                        o2=OBJECT_TEAMMATE_7;
                o3=OBJECT_TEAMMATE_8;
        }
        else if(o1==OBJECT_TEAMMATE_8)
        {
    if(posAgent.getX()>28)
                {
                        o2=OBJECT_TEAMMATE_9;
                        o3=OBJECT_TEAMMATE_11;
                }
                else
                {
                        o2=OBJECT_TEAMMATE_9;
                        o3=OBJECT_TEAMMATE_11;
                }
        }
        else if(o1==OBJECT_TEAMMATE_7)
        {
                if(posAgent.getX()>28)
                {
                        o2=OBJECT_TEAMMATE_9;
                        o3=OBJECT_TEAMMATE_10;
                }
                else
                {
                        o2=OBJECT_TEAMMATE_9;
                        o3=OBJECT_TEAMMATE_10;
                }
        }
        else if(o1==OBJECT_TEAMMATE_9)
            {
                o2=OBJECT_TEAMMATE_11;
                o3=OBJECT_TEAMMATE_10;

        }
        else return o1;
        VecPosition  posOpp=getGlobalPosition(o2);
        AngDeg angOpp=(posOpp-posAgent).getDirection();
        if(!hasOpponent(angOpp-8,angOpp+8,dDist+3)&&isInField(posOpp))return o2;
        else
        {
                posOpp=getGlobalPosition(o3);
                angOpp=(posOpp-posAgent).getDirection();
                if(!hasOpponent(angOpp-8,angOpp+8,dDist+3)&&isInField(posOpp))return o3;
        }
        return o1;
}
  bool WorldModel::hasOpponent(AngDeg angA,AngDeg angB,double dDist)
{
        VecPosition posAgent = getAgentGlobalPosition();
                VecPosition posOpp;
                AngDeg angOpp;
                int iIndex;
                for(   ObjectT o=iterateObjectStart(iIndex,OBJECT_SET_OPPONENTS,0);
                                o!=OBJECT_ILLEGAL;
                                o=iterateObjectNext(iIndex,OBJECT_SET_OPPONENTS,0))
                {
                        posOpp=getGlobalPosition(o);
                                angOpp=(posOpp-posAgent).getDirection();
                                if(angA<=angOpp&&angOpp<=angB&&posAgent.getDistanceTo(posOpp)<dDist)
                                        return true;
                }
        iterateObjectDone(iIndex);
                if(posAgent.getX()>25)
                {
                        posOpp=getGlobalPosition(getOppGoalieType());
                        angOpp=(posOpp-posAgent).getDirection();
                        if(angA<=angOpp&&angOpp<=angB&&posAgent.getDistanceTo(posOpp)<dDist)
                                return true;
                }
        return false;
}
double    WorldModel::getBeyondLine()
{
        VecPosition posOpp;
        int iIndex;
                double dist1=0;
        double dist2=0;
        for(   ObjectT o=iterateObjectStart(iIndex,OBJECT_SET_OPPONENTS,0.5);
                        o!=OBJECT_ILLEGAL;
                        o=iterateObjectNext(iIndex,OBJECT_SET_OPPONENTS,0.5))
        {
                if(o!=getOppGoalieType())
                {
                        posOpp=getGlobalPosition(o);
                        if(dist1<posOpp.getX())
                        {
                                dist1=posOpp.getX();
                        }
                        else if(dist2<posOpp.getX())
                        {
                                dist2=posOpp.getX();
                        }
                }
        }
        iterateObjectDone(iIndex);
                return (dist1+dist2)/2-3;
}
VecPosition   WorldModel::getMyPoint()
{
        VecPosition posPoint=getStrategicPosition();
        VecPosition posAgent = getAgentGlobalPosition();
                VecPosition posBall=getGlobalPosition(OBJECT_BALL);
                VecPosition posRet;
        VecPosition posOpp;
        int iIndex;
                double dist;
        for(   ObjectT o=iterateObjectStart(iIndex,OBJECT_SET_OPPONENTS,0.5);
                        o!=OBJECT_ILLEGAL;
                        o=iterateObjectNext(iIndex,OBJECT_SET_OPPONENTS,0.5))
        {
                posOpp=getGlobalPosition(o);
                dist=posPoint.getDistanceTo(posOpp);
                if(dist<6)
                {
                        posRet.setX(0.1*posBall.getX()+0.9*posOpp.getX());
                        posRet.setY(0.1*posBall.getY()+0.9*posOpp.getY());
                        return posRet;
                }
        }
        iterateObjectDone(iIndex);
                return posAgent;
}
int WorldModel::GetBestPass(int &m,int &n,int angel1,int angel2,int dist1,int dist2)
{
	int m1,m2,m3,m4,m5;
	for(int i=angel1;i<angel2+1;i++)
		for(int j=dist1;j<dist2+1;j++)
			PassValue[i][j]=0;
	for(int i=angel1;i<angel2+1;i++)
	{
		m1=(i+14),m2=(i+15),m3=(i+16);
		for(int j=dist1;j<dist2+1;j++)
		{
			if(DecitionWe[m2][j]>0&&DecitionTheir[m2][j]==0)
			PassValue[i][j]+=1+DecitionWe[m2][j]+DecitionWe[m1][j]+DecitionWe[m3][j]-(DecitionTheir[m1][j]+DecitionTheir[m3][j]);
			else if(DecitionTheir[m2][j]>0)
			PassValue[i][j]+=-1-DecitionTheir[m2][j]+DecitionWe[m1][j]+DecitionWe[m3][j]-(DecitionTheir[m1][j]+DecitionTheir[m3][j]);
			else 
			PassValue[i][j]+=DecitionWe[m1][j]+DecitionWe[m3][j]-(DecitionTheir[m1][j]+DecitionTheir[m3][j]);
		}
	}
	int MaxValue=-100,tempValue,tempM,tempN;	
	for(int i=angel1;i<angel2+1;i++)
	{
		tempValue=-100;
		m2=(i+14),m3=(i+15),m4=(i+16);
		for(int j=dist1;j<dist2+1;j++)
		{
			if(DecitionTheir[m2][j]+DecitionTheir[m3][j]+DecitionTheir[m4][j]>0)
			{
				tempValue=-100;
				break;
			}
			else if(tempValue<PassValue[i][j])
			{
				tempValue=PassValue[i][j];
				tempM=i,tempN=j;
			}
		}
		if(MaxValue<tempValue)
		{