sensory.cpp

RoboCup仿真组世界冠军源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "Types.h"
#include "Sensory.h"
#include "utils.h"
#include "Global.h"
#include "Params.h"


/*****************************  Sensory  *******************************/
Sensory::Sensory(){
	latestsenseinfo = 0;
	LastSightTime = -1;
	LastSenseTime = -1;
	PrevSenseTime = -1;
	LastSoundTime = -1;
	LastClosestMarker = No_Marker;
	dir_time = -1;
	for(int i = 0; i < CP_num_divisions; i ++){
		dir_conf[i] = 0.0f;
	}
}
/*****************************  Parse  **********************************/
/*	This Parse Module is transgrafted from CMU99 publication code.	We owe special
	thanks to CMU's great effort	*/
void Sensory::Parse(char *SensoryInfo)
{
	SenseType sense_type;
	Time time;

	PrevInterruptTime = LatestTime;
		
	switch ( SensoryInfo[3] ){
	case 'e': sense_type = See_Msg;   break; /* see   */
	case 'n': sense_type = Sense_Msg; break; /* sense */
	case 'r': 
		switch(SensoryInfo[1]){
		case 's': sense_type = SP_Msg;    break; /* server params */
		default: sense_type = No_Msg;			/* maybe (error,warning...) */
		}
		break;	
	case 'a':								 /* hear, player params or player types */
		switch( SensoryInfo[1]){
		case 'h': sense_type = Hear_Msg;  break; /* hear */
		case 'p': /* player params or player types */
			switch(SensoryInfo[8]){
			case 'p': sense_type = PP_Msg; break; /* palyer params */
			case 't': sense_type = PT_Msg; break; /* player types */
			default : sense_type = No_Msg;
			}
			break;
		default: sense_type = No_Msg;
		}
		break;
	case 'o':
		sense_type = SC_Msg; break;
	default:  sense_type = No_Msg;
	}

	switch ( sense_type ){
	/* see */
	case See_Msg:
		// extract time info
		time = get_int(&SensoryInfo);
		PrevSightTime = LastSightTime;
		LastSightTime = time;
		LatestTime = time > LatestTime ? time : LatestTime;

		/* Don't parse a second sight from the a cycle prior to the latest one*/
		if (time <= PrevSightTime) break;/*&& !situation.ClockStopped*///abort sight info when clock stopped
		 
		Parse_Sight(time, SensoryInfo);

		if(TwiceSight) Reset();//too much may result int out of array limit

		if(NewSight) TwiceSight = true;
		else NewSight = true;
    break;

	/* sense */
	case Sense_Msg: 
		// extract time info
		time = get_int(&SensoryInfo);
		
		LatestTime = time > LatestTime ? time : LatestTime;

		Parse_Sense(time, SensoryInfo);
		LastSenseTime = time;
		NewSenseInfo = true;
    break;

	/* hear */
	case Hear_Msg:  
		// extract time info
		time = get_int(&SensoryInfo);
		LatestTime = time > LatestTime ? time : LatestTime;
		Parse_Sound(time, SensoryInfo); 
		LastSoundTime = time;
		NewSound = true;
    break;
	case SP_Msg:
		Parse_ServerParams(SensoryInfo + 13);
	break;
	case PP_Msg:
		Parse_PlayerParams(SensoryInfo +13);
	break;
	case PT_Msg:
		Parse_PlayerTypes(SensoryInfo + 12);
		break;
	case SC_Msg://score
		break;
	}

	LastSenseType = sense_type;
}

void Sensory::Parse_Sense(Time time, char *SenseInfo)
{
	get_word(&SenseInfo);  
	SenseInfo += 10;       /* "view_mode " */
	switch ( SenseInfo[0] )
	{	
	case 'h': ViewQuality = VQ_High; break;  /* high */
	case 'l': ViewQuality = VQ_Low;  break;  /* low  */
	default:  my_error("Unknown view quality");
	}

	LastViewWidth = ViewWidth;
	get_next_word(&SenseInfo);

	switch ( SenseInfo[1] )
	{
	case 'o': ViewWidth = VW_Normal; break;  /* normal */
	case 'a': ViewWidth = VW_Narrow; break;  /* narrow */
	case 'i': ViewWidth = VW_Wide;   break;  /* wide   */
	default:  my_error("Unknown view quality");
	}

	float stamina = get_float(&SenseInfo); /* stamina */
	float effort  = get_float(&SenseInfo); /* effort */
	float speed   = get_float(&SenseInfo); /* speed */
	float rel_spd_angle = get_float(&SenseInfo); /* rel_spd_angle */
	float head_angle = get_float(&SenseInfo); /* head angle */

	int kicks  =   get_int(&SenseInfo); /* kicks */
	int dashes =   get_int(&SenseInfo); /* dashes */
	int turns  =   get_int(&SenseInfo); /* turns */
	int says   =   get_int(&SenseInfo); /* says */
	int turn_necks   =   get_int(&SenseInfo); /* turn_necks */

	SetMySensedInfo(ViewWidth, ViewQuality, stamina,effort,speed,rel_spd_angle,head_angle,kicks,dashes,turns,says,turn_necks,time);
}

#define NOCHNGINFO -500
#define NOFACEINFO -500

void Sensory::Parse_Sight(Time time, char *SightInfo)
{ 
	float dist,ang, dirChng, distChng;
	ObjType object_type;
	char player_side;
	bool IsGoalie ;
	UNum player_number;
	float facedir, neckdir;
	MarkerType marker = No_Marker;
	SideLine  line = SL_No_Line;
	VIEWQUALITY view_qual;
	MarkerType closestMarker = No_Marker;
	bool processThisMarker;
	float closestMarkerDist;

	/* Reset some values */
	num_seenlines.Setdata(0, time);
	num_seenmarkers.Setdata(0, time);
	LastClosestMarker = GetClosestMarker(sensory.PrevSightTime);
	ClosestMarker.Setdata(No_Marker, time);

	while (*SightInfo != ')'){

		dirChng = NOCHNGINFO;
		facedir = NOFACEINFO;
		neckdir = NOFACEINFO;
		player_number = player_side = 0;

		get_word(&SightInfo);        /* " ((" */

		if ( *SightInfo=='g' ){
			
			object_type = OBJ_Marker;/* it is  a "goal " */
			SightInfo += 2;
			if ( *SightInfo=='r' )			marker = Goal_R; 
			else if ( *SightInfo=='l' )     marker = Goal_L; 
			else my_error("goal ?");
		}
		else if ( *SightInfo=='G' ){
			object_type = OBJ_Marker_Behind;
			marker = ClosestGoal();
		} 
		else if ( *SightInfo=='f' ){
			
			object_type = OBJ_Marker;
			SightInfo += 2;          /* it is a "flag " */
			if ( *SightInfo=='r' )
			{
				SightInfo += 2;
				if ( *SightInfo=='0' )		marker = Flag_R0;   
				else if ( *SightInfo=='b' )
				{
					SightInfo += 1;
			    	if ( *SightInfo==')' )  marker = Flag_RB; 
					else
					{
						SightInfo += 1;
						if ( *SightInfo=='1' )		  marker = Flag_RB10; 
						else if ( *SightInfo=='2' )   marker = Flag_RB20; 
						else if ( *SightInfo=='3' )   marker = Flag_RB30; 
				    	else my_error("flag r b ?");
					}
				}
			else if ( *SightInfo=='t' )
			{	
				SightInfo+=1;
				if ( *SightInfo==')' )     marker = Flag_RT; 
				else
				{	SightInfo+=1;
					if ( *SightInfo=='1' )   marker = Flag_RT10; 
					else if ( *SightInfo=='2' )   marker = Flag_RT20; 
					else if ( *SightInfo=='3' )   marker = Flag_RT30; 
					else my_error("flag r t ?");
					}
			}
			else my_error("flag r ?");
			} 
			else if ( *SightInfo=='l' )
			{	
				SightInfo+=2;
				if ( *SightInfo=='0' )       marker = Flag_L0;   
				else if ( *SightInfo=='b' )
				{	
					SightInfo+=1;
			    	if ( *SightInfo==')' )     marker = Flag_LB; 
					else
					{	
						SightInfo+=1;
						if ( *SightInfo=='1' )   marker = Flag_LB10; 
		    			else if ( *SightInfo=='2' )   marker = Flag_LB20; 
			    		else if ( *SightInfo=='3' )   marker = Flag_LB30; 
				    	else my_error("flag l b ?");
					}
				}
				else if ( *SightInfo=='t' )
				{	
					SightInfo+=1;
    				if ( *SightInfo==')' )     marker = Flag_LT; 
	    			else
					{
						SightInfo+=1;
		    			if ( *SightInfo=='1' )   marker = Flag_LT10; 
			    		else if ( *SightInfo=='2' )   marker = Flag_LT20; 
				    	else if ( *SightInfo=='3' )   marker = Flag_LT30; 
				    	else my_error("flag l t ?");
					}  
				} 
				else my_error("flag l ?");
			} 
     		else if ( *SightInfo=='t' )
			{
				SightInfo+=2;
				if ( *SightInfo=='0' )       marker = Flag_T0;   
				else if ( *SightInfo=='l' )
				{
					SightInfo+=2;	
					if ( *SightInfo=='1' )     marker = Flag_TL10; 
					else if ( *SightInfo=='2' )     marker = Flag_TL20; 
					else if ( *SightInfo=='3' )     marker = Flag_TL30; 
					else if ( *SightInfo=='4' )     marker = Flag_TL40; 
					else if ( *SightInfo=='5' )     marker = Flag_TL50; 
					else my_error("flag t l ?");
				}
				else if ( *SightInfo=='r' )
				{	
					SightInfo+=2;
		    		if ( *SightInfo=='1' )     marker = Flag_TR10; 
		        	else if ( *SightInfo=='2' )     marker = Flag_TR20;   
		        	else if ( *SightInfo=='3' )     marker = Flag_TR30; 
		        	else if ( *SightInfo=='4' )     marker = Flag_TR40; 
		        	else if ( *SightInfo=='5' )     marker = Flag_TR50; 
        			else my_error("flag t r ?");
				} 
				else my_error("flag t ?");
			}
			else if ( *SightInfo=='b' )
			{	
				SightInfo+=2;
				if ( *SightInfo=='0' )       marker = Flag_B0;   
				else if ( *SightInfo=='l' )
				{
					SightInfo+=2;
					if ( *SightInfo=='1' )     marker = Flag_BL10; 
					else if ( *SightInfo=='2' )     marker = Flag_BL20;
					else if ( *SightInfo=='3' )     marker = Flag_BL30;
					else if ( *SightInfo=='4' )     marker = Flag_BL40; 
					else if ( *SightInfo=='5' )     marker = Flag_BL50; 
        		    else my_error("flag b l ?");  
				} 
				else if ( *SightInfo=='r' )
				{	
					SightInfo+=2;
				    if ( *SightInfo=='1' )     marker = Flag_BR10; 
			    	else if ( *SightInfo=='2' )     marker = Flag_BR20; 
		    		else if ( *SightInfo=='3' )     marker = Flag_BR30; 
			    	else if ( *SightInfo=='4' )     marker = Flag_BR40; 
			    	else if ( *SightInfo=='5' )     marker = Flag_BR50; 
		    		else my_error("flag b r ?");
				} 
		    	else my_error("flag b ?");
			} 
			else if ( *SightInfo=='c' )	
			{
				SightInfo+=1;
				if ( *SightInfo==')' )       marker = Flag_C;
                else 
				{
					SightInfo+=1;