debugagent.cpp

浙江大学 RoboCup3D 2006 源代码

/***************************************************************************
 *   Copyright (C) 2004 - 2006 by ZJUBase                                  *
 *                                National Lab of Industrial Control Tech. *
 *                                Zhejiang University, China               *
 *                                                                         *
 *   Team members:                                                         *
 *    Currently the team leader is,                                        *
 *           Hao JIANG ([e-mail]; [e-mail])       *
 *    In the next season, the leader will be                               *
 *           Yifeng ZHANG ([e-mail])                        *
 *    ZJUBase 3D agent is created by                                       *
 *           Dijun LUO ([e-mail])                             *
 *    All the members who has ever contributed:                            *
 *           Jun JIANG                                                     *
 *           Xinfeng DU ([e-mail])                             *
 *           Yang ZHOU ([e-mail])                             *
 *           Zhipeng YANG                                                  *
 *           Xiang FAN                                                     *
 *                                                                         *
 *   Team Manager:                                                          *
 *      Ms. Rong XIONG ([e-mail])                            *
 *                                                                         *
 *   If you met any problems or you have something to discuss about        *
 *   ZJUBase. Please feel free to contact us through EMails given below.   *
 *                                                                         *
 *   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; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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 General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "stdafx.h"

#include "netif.h"
#include "../ZJUBase/Brain.h"
#include "../ZJUBase/ActionSystem.h"
#include "../ZJUBase/Strategy.h"

#include <sstream>
// The one and only application object

CWinApp theApp;
vector<string> strDebug;
vector<string> geoDebug;
using namespace std;

#define for if(0);else for

void ParseServerSettings(char *str)
{
	double FieldLength,FieldWidth,FieldHeight,GoalWidth,GoalDepth,GoalHeight;
	sscanf(str,"(Init (FieldLength %lf)(FieldWidth %lf)(FieldHeight %lf)(GoalWidth %lf)(GoalDepth %lf)(GoalHeight %lf)",
		&FieldLength,
		&FieldWidth,
		&FieldHeight,
		&GoalWidth,
		&GoalDepth,
		&GoalHeight);
	ss.mFieldLength = FieldLength;
	ss.mFieldWidth = FieldWidth;
	ss.mFieldHeight = FieldHeight;
	ss.mGoalWidth = GoalWidth;
	ss.mGoalDepth = GoalDepth;
	ss.mGoalHeight = GoalHeight;
    strategy.Init(&ss);
}
/*
N 4
T 0.29
M 2
S 11
O -16.0807 -5.01563 0.22
V -0.0145945 -0.0086559 0
o -16.083 -5.01699 0
v -0.00876806 -0.00521677 0
pantile -0.204293 -0.00910117
P 1 1 -51.0129 -0.0914512 0.317994
P 1 2 -13.4561 18.0955 0.208374
P 1 3 -16.0081 4.80064 0.226703
P 1 4 -16.0501 -5.0867 0.223298
P 1 5 -13.5319 -18.0756 0.232921
P 1 6 -17.1352 0.516615 0.62707
P 1 7 -2.04962 12.0209 0.117536
P 1 8 -2.09655 -12.1029 0.220905
P 1 9 -16.0334 -0.70281 0.59653
P 1 10 -1.96481 20.8939 0.22697
P 1 11 -2.08459 -20.9196 0.212631
P 2 1 52.9491 -0.225089 0.526939
P 2 2 25.0146 19.9765 0.240169
P 2 3 24.9062 -0.133892 0.213136
P 2 4 24.8963 -20.0773 0.228803
P 2 5 18.6825 28.0063 0.0520357
P 2 6 18.7995 9.47775 0.163367
P 2 7 18.6445 -9.65993 0.223856
P 2 8 18.6341 -28.1274 0.322227
P 2 9 10.147 4.67918 0.259542
P 2 10 9.89542 -0.0198526 0.164891
P 2 11 10.0348 -4.70161 0.227589
B -0.0390305 -0.0262478 0.0944295 0.032468 -0.0121922 -0.0178365
*/

void ParseWorldModel(char *str)
{
	istringstream is(str);
	int len = strlen(str);
	string s;
	int pm;
	is>>s;	is>>global.wm.myNumber;
	is>>s;	is>>global.wm.currentTime;
	is>>s;	is>>pm;
	global.wm.pm = (TPlayMode)pm;
	is>>s;	is>>global.wm.mSensationCount;
	is>>s;	is>>global.wm.myPos.x>>global.wm.myPos.y>>global.wm.myPos.z;
	is>>s;	is>>global.wm.myVel.x>>global.wm.myVel.y>>global.wm.myVel.z;
	is>>s;	is>>global.wm.nextPos.x>>global.wm.nextPos.y>>global.wm.nextPos.z;
	is>>s;	is>>global.wm.nextVel.x>>global.wm.nextVel.y>>global.wm.nextVel.z;
	is>>s;  is>>global.wm.myPanAngle>>global.wm.myTiltAngle;
	for(int i=0;i<11;i++){
		is>>s>>s>>s;
		is>>global.wm.ourPos[i].x>>global.wm.ourPos[i].y>>global.wm.ourPos[i].z;
		is>>s>>s>>s;
		is>>global.wm.ourVel[i].x>>global.wm.ourVel[i].y>>global.wm.ourVel[i].z;
	}
	for(int i=0;i<11;i++){
		is>>s>>s>>s;
		is>>global.wm.oppPos[i].x>>global.wm.oppPos[i].y>>global.wm.oppPos[i].z;
		is>>s>>s>>s;
		is>>global.wm.oppVel[i].x>>global.wm.oppVel[i].y>>global.wm.oppVel[i].z;
	}
	is>>s;
	is>>global.wm.ballPos.x>>global.wm.ballPos.y>>global.wm.ballPos.z;
	is>>global.wm.ballVel.x>>global.wm.ballVel.y>>global.wm.ballVel.z;

	is >> s;
	for (int i = 0; i < 8; ++i) {
		is >> global.wm.noflag[i];
	}
	for (int i = 0; i < 11; ++i) {
		is >> global.wm.noour[i];
	}
	for (int i = 0; i < 11; ++i) {
		is >> global.wm.noopp[i];
	}
	is >> global.wm.noball;
}
void ParseMonitor(char* str)
{
	
}

int APIENTRY WinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPSTR     lpCmdLine,
                     int       nCmdShow)
{
		// TODO: code your application's behavior here.
		Network::sock = Network::init_connection("localhost",8001);
		char buff[4096];
		Network::send_message("init",Network::sock);
		Situation st;
		Brain brain;
		predictor.Init(&ss);
		st.Init(&ss);
		brain.Init(&ss,&st);
		as.Init(&ss,&st);
		while(true){
			Network::receive_message(buff,Network::sock);
			if(buff[0]=='('&&buff[1]=='I'&&buff[2]=='n'&&buff[3]=='i'&&buff[4]=='t')
				ParseServerSettings(buff);
			if(buff[0]=='('&&buff[1]=='I'&&buff[2]=='n'&&buff[3]=='f'&&buff[4]=='o'){

				ParseMonitor(buff);
				strDebug.clear();
				geoDebug.clear();
				/*
				ShowCircle(10,10,0.111,"255 255 0 1");
				ShowSphere(10,12,0.111,"255 255 0 1");
				ShowLine(0,0,0,10,10,2,"10 10 10");
				ShowPie(10,10,30,10,50,"255 0 0 2");
				ShowRect(10,10,30,20,"255 0 0 1");
				*/
				ShowCircle(global.wm.ballPos,0.111,"0 0 0 3");
				ShowCircle(global.wm.myPos,0.22,"200 255 0 3");

				brain.Think();
				int i;
				int num;
				num = global.nCommand;
				string s;
				for(i=0;i<num;i++){
					char tmp[1024];
					s = as.GenerateCommandString(i);
					strcpy(tmp,s.c_str());
					ShowMessage(tmp);
				}
				ShowMessage("Think done");
				Network::send_message("!",Network::sock);
			}
			if(buff[0]=='N')
				ParseWorldModel(buff);
			buff[0]=0;
            Sleep(1);
		}

	return 0;
}

void ShowLine(Vector3 p1,Vector3 p2,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"L %3.5f %3.5f %3.5f %3.5f %3.5f %3.5f %s",p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,buf);
	Network::send_message(msg,Network::sock);
}
void ShowDirection(Vector3 p1,Vector3 p2,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"L %3.5f %3.5f %3.5f %3.5f %3.5f %3.5f %s",p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,buf);
	Network::send_message(msg,Network::sock);
	Vector3 pp1,pp2;
	double dist = (p1 - p2).mod();
	Angle ang = (p2-p1).ang();
	Vector3 p(dist,ang,0,POLAR);
	p *= 0.04;
	pp1 = p;
	pp1.rotate2d(135);
	pp1 += p2;
	ShowLine(pp1,p2,buf);
	pp2 = p;
	pp2.rotate2d(-135);
	pp2 += p2;
	ShowLine(pp2,p2,buf);
}

void ShowLine(Vector3 p0,double dist,double ang,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	Vector3 p1(p0);
	Vector3 p(dist,ang,0,POLAR);
	Vector3 p2=p1+p;
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"L %3.5f %3.5f %3.5f %3.5f %3.5f %3.5f %s",p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,buf);
	Network::send_message(msg,Network::sock);
}

void ShowDirection(Vector3 p0,double dist,double ang,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	Vector3 p1(p0);
	Vector3 p(dist,ang,0,POLAR);
	Vector3 p2=p1+p;
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"L %3.5f %3.5f %3.5f %3.5f %3.5f %3.5f %s",p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,buf);
	Network::send_message(msg,Network::sock);
	Vector3 pp1,pp2;
	p *= 0.04;
	pp1 = p;
	pp1.rotate2d(135);
	pp1 += p2;
	ShowLine(pp1,p2,buf);
	pp2 = p;
	pp2.rotate2d(-135);
	pp2 += p2;
	ShowLine(pp2,p2,buf);
}

void ShowLine(double x1,double y1,double z1,double x2,double y2,double z2,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"L %3.5f %3.5f %3.5f %3.5f %3.5f %3.5f %s",x1,y1,z1,x2,y2,z2,buf);
	Network::send_message(msg,Network::sock);
}
void ShowSphere(double x,double y,double z,double r,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"S %3.5f %3.5f %3.5f %3.5f %s",x,y,z,r,buf);
	Network::send_message(msg,Network::sock);
}
void ShowCircle(double x,double y,double r,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"C %3.5f %3.5f %3.5f %s",x,y,r,buf);
	Network::send_message(msg,Network::sock);

}
void ShowCircle(Vector3 p0,double r,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"C %3.5f %3.5f %3.5f %s",p0.x,p0.y,r,buf);
	Network::send_message(msg,Network::sock);

}
void ShowRect(double x1,double y1,double x2,double y2,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"R %3.5f %3.5f %3.5f %3.5f %s",x1,y1,x2,y2,buf);
	Network::send_message(msg,Network::sock);

}
void ShowPie(double x,double y,double r,double ang1,double ang2,char *str, ...)
{
	char msg[1024];
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	sprintf(msg,"P %3.5f %3.5f %3.5f %3.5f %3.5f %s",x,y,r,ang1,ang2,buf);
	Network::send_message(msg,Network::sock);

}

void ShowMessage( char *str, ... )
{
	char buf[2000];
	va_list ap;
	va_start( ap, str );
	vsprintf( buf, str, ap );
	va_end(ap);
	string s = "M ";
	s += buf;
	strcpy(buf,s.c_str());
	Network::send_message(buf,Network::sock);
}

void Paint()
{
	char msg[1024]="!";
	Network::send_message(msg,Network::sock);
}