清华大学机器人系统源代码 vc6.0

/* 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 yjy01@mails.tsinghua.edu.cn. */ #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;