main.cpp

浙江大学 RoboCup3D 2006 源代码

				chooseAgent1 = false;
			if(chooseAgent1)
			{
				//get agentPos
				agentPos[count]= agent1Pos[count];
				//get panAngle
				panAngleFromDist[2*count] = agent1Theta - data[pt1].theta - 180;
				panAngleFromDist[2*count+1] = (agentPos[count]-vFlagReal[pt2]).ang() - data[pt2].theta - 180;
				//get coefficient of sigma_r0,sigma_r1(sigma_dist) for x and y
				px_r0 = dx * data[pt1].distance / dist_pt1_pt2 - dy * (1 - a /dist_pt1_pt2) * data[pt1].distance / h;
				px_r1 = - dx * data[pt2].distance / dist_pt1_pt2 - dy * (a / dist_pt1_pt2) * data[pt2].distance / h;
				py_r0 = dy * data[pt1].distance / dist_pt1_pt2 + dx * (1 - a /dist_pt1_pt2) * data[pt1].distance / h;
				py_r1 = - dy * data[pt2].distance / dist_pt1_pt2 + dx * (a / dist_pt1_pt2) * data[pt2].distance / h;				
			}
			else
			{
				//get agentPos
				agentPos[count] = agent2Pos[count];
				//get panAngle
				panAngleFromDist[2*count] =  agent2Theta - data[pt1].theta - 180;
				panAngleFromDist[2*count+1] = (agentPos[count]-vFlagReal[pt2]).ang() - data[pt2].theta - 180;
				//get coefficient of sigma_dist for x and y
				px_r0 = dx * data[pt1].distance /dist_pt1_pt2 + dy * (1 - a /dist_pt1_pt2) * data[pt1].distance / h;
				px_r1 =  - dx * data[pt2].distance / dist_pt1_pt2 + dy * (a / dist_pt1_pt2) * data[pt2].distance / h;
				py_r0 = dy * data[pt1].distance / dist_pt1_pt2 - dx * (1 - a /dist_pt1_pt2) * data[pt1].distance / h;
				py_r1 =  - dy * data[pt2].distance / dist_pt1_pt2 - dx * (a / dist_pt1_pt2) * data[pt2].distance / h;
			}
			
			//get panAngle from -180 to 180
			panAngleFromDist[2*count] = Normalize(panAngleFromDist[2*count]);
			panAngleFromDist[2*count+1] = Normalize(panAngleFromDist[2*count+1]);
			if(fabs(global.wm.myPanAngle) > 170){
				if(panAngleFromDist[2*count] <0){
					panAngleFromDist[2*count] = panAngleFromDist[2*count] +360;
				}
				if(panAngleFromDist[2*count+1] <0){
					panAngleFromDist[2*count+1] = panAngleFromDist[2*count+1] +360;
				}
			}
			
			//get sigma of x and y
			sigmaX[count] = (px_r0*px_r0 *sigma_dist*data[pt1].distance/100 + px_r1*px_r1*sigma_dist*data[pt2].distance/100);
			sigmaY[count] = (py_r0*py_r0 *sigma_dist*data[pt1].distance/100 + py_r1*py_r1*sigma_dist*data[pt2].distance/100);
			sumX_deno = sumX_deno + 1/sigmaX[count];
			sumY_deno = sumY_deno + 1/sigmaY[count];
			sumX_nume = sumX_nume + 1/sigmaX[count]*agentPos[count].x;
			sumY_nume = sumY_nume + 1/sigmaY[count]*agentPos[count].y;
			//get coefficient of sigma_x sigma_y for panAngle
			double tmpTheta1,tmpTheta2;
			tmpTheta1 = (agentPos[count].x - vFlagReal[pt1].x )/(sqrt(pow((agentPos[count].x - vFlagReal[pt1].x),2)+pow((agentPos[count].y - vFlagReal[pt1].y),2)));
			tmpTheta2 = (agentPos[count].x - vFlagReal[pt1].x )/(sqrt(pow((agentPos[count].x - vFlagReal[pt1].x),2)+pow((agentPos[count].y - vFlagReal[pt1].y),2)));
			double tmpPanxy1, tmpPanxy2;
			tmpPanxy1 = (180/PI)*(1/sqrt(1+pow(tmpTheta1,2)))*(1/(2*tmpTheta1))*(1/pow(tmpTheta1,2))*2;
			tmpPanxy2 = (180/PI)*(1/sqrt(1+pow(tmpTheta2,2)))*(1/(2*tmpTheta2))*(1/pow(tmpTheta2,2))*2;
			pan_x[2*count] = tmpPanxy1*(pow((agentPos[count].y-vFlagReal[pt1].y),2)/pow((agentPos[count].x-vFlagReal[pt1].x),3));
			pan_x[2*count+1] = tmpPanxy2*(pow((agentPos[count].y-vFlagReal[pt2].y),2)/pow((agentPos[count].x-vFlagReal[pt2].x),3));
			pan_y[2*count] = tmpPanxy1*((agentPos[count].y-vFlagReal[pt1].y)/pow((agentPos[count].x-vFlagReal[pt1].x),2));
			pan_y[2*count+1] = tmpPanxy2*((agentPos[count].y-vFlagReal[pt2].y)/pow((agentPos[count].x-vFlagReal[pt2].x),2));
			//get sigma of panAngle
			sigmaPan[2*count] = sigma_theta + pan_x[2*count]*pan_x[2*count]*sigmaX[count] + pan_y[2*count]*pan_y[2*count]*sigmaY[count];
			sigmaPan[2*count+1] = sigma_theta + pan_x[2*count+1]*pan_x[2*count+1]*sigmaX[count] + pan_y[2*count+1]*pan_y[2*count+1]*sigmaY[count];
			sumPan_deno = sumPan_deno + 1/sigmaPan[2*count] + 1/sigmaPan[2*count+1];
			sumPan_nume = sumPan_nume + 1/sigmaPan[2*count]*panAngleFromDist[2*count] + 1/sigmaPan[2*count+1]*panAngleFromDist[2*count+1];

			//cerr << "agent1Pos(" << agent1Pos[count].x << "," << agent1Pos[count].y << ")"<< endl;
			//cerr << "agent2Pos(" << agent2Pos[count].x << "," << agent2Pos[count].y << ")"<< endl;
			//cerr << "vMyPosFromFlag(" << vMyPosFromFlag[pt1].x << "," << vMyPosFromFlag[pt1].y << ")"<< endl;
			//cerr << "agentPos(" << agentPos[count].x << "," << agentPos[count].y << ")"<< endl;

			count ++;
		}
	}

	
	Vector3 myPos(0,0,0);
	Vector3 oneFlagPos;
	int flag = 0;

	for(i = 0; i < 8; i++){
		if(fabs(vRelPos[i].x)<EPS&&fabs(vRelPos[i].y)<EPS&&fabs(vRelPos[i].z)<EPS)
			continue;
		oneFlagPos = vMyPosFromFlag[i];
		flag++;
	}
	//cerr << "flag: " << flag << endl;
	//cerr << "count: " << count << endl;
	
	//calc panAngle
	//cerr << "pre_myPanAngle: " << global.wm.myPanAngle << endl;
	if(count > 0 && flag >= 2){
		double tmpPan;
		tmpPan = sumPan_nume / sumPan_deno;
		if(fabs(tmpPan - global.wm.myPanAngle) < 20)
			global.wm.myPanAngle = Normalize(tmpPan);
		//cerr << "myPanAngle: " << global.wm.myPanAngle << endl;
	}
	if(count == 0 && flag == 0 && global.wm.currentTime >0)
		global.wm.myPanAngle = global_previous[0].wm.myPanAngle;
	if(isnan(global.wm.myPanAngle))
		global.wm.myPanAngle = 0;

	//calc tiltAngle
	Angle tmpAngle;
	for(i = 0; i < 8; i++){
		if(fabs(vRelPos[i].x)<EPS&&fabs(vRelPos[i].y)<EPS&&fabs(vRelPos[i].z)<EPS)
			continue;
		tmpAngle = -data[i].phi - Rad2Deg(asin(0.22/data[i].distance));
		Normalize(tmpAngle);
		//get coefficient of sigma_dist for tiltAngle
		tilt_dist = 180*0.22/(sqrt(1-pow(0.22/data[i].distance,2))*data[i].distance*PI);
		if(isnan(tilt_dist))
			continue;
		//get sigma of tiltAngle
		sigmaTilt[i] = sigma_phi + tilt_dist*tilt_dist*sigma_dist*data[i].distance/100;
		sumTilt_deno = sumTilt_deno + 1/sigmaTilt[i];
		sumTilt_nume = sumTilt_nume + 1/sigmaTilt[i]*tmpAngle;
	}
	if(i >= 1){
		global.wm.myTiltAngle = sumTilt_nume / sumTilt_deno;
		//cerr << "myTiltAngle: " << global.wm.myTiltAngle << endl;
	}
	if(flag == 0 && global.wm.currentTime >0){
		global.wm.myTiltAngle = global_previous[0].wm.myTiltAngle;
	}
	if(isnan(global.wm.myTiltAngle))
		global.wm.myTiltAngle = 0;	
	
	
	//get agent position
	if(flag == 0)
		myPos=global.wm.nextPos;
	if(flag == 1)			
		myPos =Vector3(oneFlagPos.x,oneFlagPos.y,ss.playerSize);
	if(flag >= 2){
		Vector3 agentVec=0;
		agentVec.x = sumX_nume/sumX_deno;
		agentVec.y = sumY_nume/sumY_deno;
		myPos =Vector3(agentVec.x, agentVec.y, ss.playerSize);
	}

	//no solution while calcing the agentPos
	if(count == 0 && flag > 0){
		Vector3 agentVec=0;
		int flagCount=0;
		for(i = 0; i < 8; i++){
			if(fabs(vRelPos[i].x)<EPS&&fabs(vRelPos[i].y)<EPS&&fabs(vRelPos[i].z)<EPS)
				continue;
			agentVec += vMyPosFromFlag[i];
			++flagCount;
		}
		agentVec = agentVec / flagCount;
		myPos = Vector3(agentVec.x, agentVec.y, ss.playerSize);		
	}
	//cerr << "myPanAngle: " << global.wm.myPanAngle << endl;
	
//my Pos	
	g_wm->mMyPos = Vector3(myPos.x,myPos.y,myPos.z);
	global.wm.myPos = myPos;//V2V(wm->GetMyPosition());
	
//=================================================
//play mode	
	global.wm.pm = g_wm->GetPlayMode();
//current time
	global.wm.currentTime = g_wm->mTime;
	//cerr<<"T: "<<global.wm.currentTime<<endl;
//ball pos
	WorldModel::VisionSense ballSense = g_wm->GetVisionSense(WorldModel::VO_BALL);
	ballSense.theta += global.wm.myPanAngle;
	ballSense.phi += global.wm.myTiltAngle;

	Vector3 ballPos;
       if(fabs(ballSense.distance)<EPS){
	       ballPos=global.wm.ballPos+global.wm.ballVel;
		global.wm.noball++;
	}
	else{
    	       ballPos = g_wm->GetPosition(ballSense);
		global.wm.noball=0;
	}
	//cerr<<"noball="<<global.wm.noball<<endl;
	global.wm.ballPos = V2V(ballPos);
//	cerr<<global.wm.ballPos.x-global.wm.myPos.x<<" "<<global.wm.ballPos.y-global.wm.myPos.y<<endl;

//========
	Vector3 pos;
//teammate
	WorldModel::VisionSense teamMateSense;
	for(i=0;i<11;i++){
		if(i == global.wm.myNumber-1)
			continue;
	       teamMateSense = g_wm->GetVisionSense((WorldModel::VisionObject)(WorldModel::VO_OUR1+i));
		teamMateSense.theta+=global.wm.myPanAngle;
		teamMateSense.phi += global.wm.myTiltAngle;

		if(teamMateSense.distance<EPS)
		   	pos=global.wm.ourPos[i]+global.wm.ourVel[i]/6.37;
		else
			pos = g_wm->GetPosition(teamMateSense);

		global.wm.ourVel[i]=Vector3(V2V(pos)-global.wm.ourPos[i])*6.37;
		global.wm.ourVel[i].z=0;
		if(global.wm.ourVel[i].mod()>5) global.wm.ourVel[i]=Vector3(0,0,0);
//		cerr<<"ourvel"<<i<<"    "<<global.wm.ourVel[i].x<<"="<<pos.x<<"-"<<global.wm.ourPos[i].x<<endl;
		global.wm.ourPos[i] = V2V(pos);
	}
	
	if(global.wm.myNumber != 0)
		global.wm.ourPos[global.wm.myNumber - 1] = global.wm.myPos;
	
//========
//opp player
	WorldModel::VisionSense oppSense;
	for(i=0;i<11;i++){
		oppSense = g_wm->GetVisionSense((WorldModel::VisionObject)(WorldModel::VO_OPP1+i));
		oppSense.theta += global.wm.myPanAngle;
		oppSense.phi += global.wm.myTiltAngle;

		if(oppSense.distance<EPS)
		   	pos=global.wm.oppPos[i]+global.wm.oppVel[i]/6.37;
		else
			pos = g_wm->GetPosition(oppSense);

		global.wm.oppVel[i]=Vector3(V2V(pos)-global.wm.oppPos[i])*6.37;
		global.wm.oppVel[i].z=0;
		if(global.wm.oppVel[i].mod()>5) global.wm.oppVel[i]=Vector3(0,0,0);
//		cerr<<"oppvel"<<i<<"     "<<global.wm.oppVel[i].x<<"="<<pos.x<<"-"<<global.wm.oppPos[i].x<<endl;
		global.wm.oppPos[i] = V2V(pos);
	}
//========
	for(i=POINT_MAX-1;i>0;i--){
		global_previous[i] = global_previous[i-1];
	}
	
	global_previous[0] = global;
	if(global.wm.currentTime>0){
		//cerr<<"T "<<global.wm.currentTime<<endl;
		//cerr<<"obp "<<global.wm.ballPos.x<<" "<<global.wm.ballPos.y<<" "<<global.wm.ballPos.z<<endl;
		filter.filterOfBall();
		filter.filterOfAgent();
		global.wm.ourVel[global.wm.myNumber-1]=global.wm.myVel;

		//cout <<"T: " << global.wm.currentTime << "\t\t";
		//cout << "filter_myPos: " << global.wm.myPos.x<< "\t\t" << global.wm.myPos.y<<  "\t\t" << global.wm.myPos.z <<  "\t\t" << endl;

		//refresh other objects information
		g_wm->mMyPos = Vector3(myPos.x,myPos.y,myPos.z);
		global.wm.myPos = myPos;//V2V(wm->GetMyPosition());
		//ball pos
		WorldModel::VisionSense ballSense = g_wm->GetVisionSense(WorldModel::VO_BALL);
		ballSense.theta += global.wm.myPanAngle;
		ballSense.phi += global.wm.myTiltAngle;
       	if(fabs(ballSense.distance)<EPS)
	       	ballPos=global.wm.ballPos+global.wm.ballVel;
		else
			ballPos = g_wm->GetPosition(ballSense);
		global.wm.ballPos = V2V(ballPos);
		//filter.filterOfBall();

		//teammate
		WorldModel::VisionSense teamMateSense;
		for(i=0;i<11;i++){
			if(i == global.wm.myNumber-1)
				continue;
	       	teamMateSense = g_wm->GetVisionSense((WorldModel::VisionObject)(WorldModel::VO_OUR1+i));
			teamMateSense.theta += global.wm.myPanAngle;
			teamMateSense.phi += global.wm.myTiltAngle;

		////////////////////////////yzp0531
			if(teamMateSense.distance<EPS){
				global.wm.noour[i]++;

			//	cerr<<"noour "<<i<<" "<<global.wm.noour[i]<<endl;

				if(global.wm.noour[i]<=20)
					pos=global.wm.ourPos[i]+global.wm.ourVel[i]/6.37;
				else
					pos=strategy.SBSPPosition(i+1);
					
				if(fabs(Normalize((pos-global.wm.myPos).ang()-global.wm.myPanAngle))<85)
						pos=Vector3(-X(110),-Y(110),0);
			}
			else{
				pos = g_wm->GetPosition(teamMateSense);
				global.wm.noour[i]=0;
			}

			if((global.wm.ballPos-global.wm.ourPos[i]).mod()>1)
				pos.z=0.22;
				
			
			global.wm.ourVel[i]=Vector3(V2V(pos)-global.wm.ourPos[i])*6.37;
			global.wm.ourVel[i].z=0;
			if(global.wm.ourVel[i].mod()>5) global.wm.ourVel[i]=Vector3(0,0,0);
			global.wm.ourPos[i] = V2V(pos);
		}
	
		if(global.wm.myNumber != 0)
			global.wm.ourPos[global.wm.myNumber - 1] = global.wm.myPos;

		//opp player
		WorldModel::VisionSense oppSense;
		for(i=0;i<11;i++){
			oppSense = g_wm->GetVisionSense((WorldModel::VisionObject)(WorldModel::VO_OPP1+i));
			oppSense.theta += global.wm.myPanAngle;
			oppSense.phi += global.wm.myTiltAngle;

			//////////////////////////yzp0531
			if(oppSense.distance<EPS){

				global.wm.noopp[i]++;

			//	cerr<<"noopp "<<i<<" "<<global.wm.noopp[i]<<endl;

				pos=global.wm.oppPos[i]+global.wm.oppVel[i]/6.37;
				if(fabs(Normalize((pos-global.wm.myPos).ang()-global.wm.myPanAngle))<90)
						pos=Vector3(-X(110),-Y(110),0);
					
			}
			else{
				global.wm.noopp[i]=0;
				pos = g_wm->GetPosition(oppSense);
			}

			if((global.wm.ballPos-global.wm.oppPos[i]).mod()>1)
				pos.z=0.22;
			global.wm.oppVel[i]=Vector3(V2V(pos)-global.wm.oppPos[i])*6.37;
			global.wm.oppVel[i].z=0;
			if(global.wm.oppVel[i].mod()>5) global.wm.oppVel[i]=Vector3(0,0,0);
			global.wm.oppPos[i] = V2V(pos);
		}
		global_previous[0] = global;

		//cerr<<"myvel"<<"     "<<global.wm.myVel.x<<"="<<global.wm.myPos.x<<"-"<<global_previous[1].wm.myPos.x<<"="<<global.wm.myPos.x-global_previous[1].wm.myPos.x<<endl;
		//cerr<<"fbp "<<global.wm.ballPos.x<<" "<<global.wm.ballPos.y<<" "<<global.wm.ballPos.z<<endl;
		global.wm.nextPos = predictor.predictAgentPosNextCycle();
		global.wm.nextVel = predictor.predictAgentVelNextCycle();		
	}

	// update global from HearPerceptor
	updateGlobalFromHear();

	if(global.wm.currentTime>0)
		LogWM();
}

void Sensor::updateGlobalFromHear()
{
	if (global.wm.noball > 1 && global.wm.hearMsg.isBallSeen == 1) {
		global.wm.ballPos = global.wm.hearMsg.ballPos + global.wm.hearMsg.ballVel;
		global.wm.ballVel = global.wm.hearMsg.ballVel * ss.B_decay;
	}
	for (int i = 0; i < 11; ++i) {
		if (global.wm.noour[i] > 1 && global.wm.hearMsg.our[i][0] > 0.9) {
			global.wm.ourPos[i] = Vector3(global.wm.hearMsg.our[i][1], global.wm.hearMsg.our[i][2], 0.22);
		}
	}
	for (int i = 0; i < 11; ++i) {
		if (global.wm.noopp[i] > 1 && global.wm.hearMsg.opp[i][0] > 0.9) {
			global.wm.oppPos[i] = Vector3(global.wm.hearMsg.opp[i][1], global.wm.hearMsg.opp[i][2], 0.22);
		}
	}
}

void Sensor::LogWM(){
	int i;
//	cout<<"========WM========="<<endl;
	cout<<"N "<<global.wm.myNumber<<endl;
	cout<<"T "<<global.wm.currentTime<<endl;
	cout<<"M "<<global.wm.pm<<endl;
	cout<<"S "<<global.wm.mSensationCount<<endl;
	cout<<"O "<<global.wm.myPos.x<<" "<<global.wm.myPos.y<<" "<<global.wm.myPos.z<<endl;
	cout<<"V "<<global.wm.myVel.x<<" "<<global.wm.myVel.y<<" "<<global.wm.myVel.z<<endl;	
	cout<<"o "<<global.wm.nextPos.x<<" "<<global.wm.nextPos.y<<" "<<global.wm.nextPos.z<<endl;
	cout<<"v "<<global.wm.nextVel.x<<" "<<global.wm.nextVel.y<<" "<<global.wm.nextVel.z<<endl;
	cout<<"pantilt "<<global.wm.myPanAngle<<" "<<global.wm.myTiltAngle<<endl;

	for(i=0;i<11;i++){
		cout<<"P 1 "<<i+1<<" "<<global.wm.ourPos[i].x<<" "<<global.wm.ourPos[i].y<<" "<<global.wm.ourPos[i].z<<endl;
		cout<<"V 1 "<<i+1<<" "<<global.wm.ourVel[i].x<<" "<<global.wm.ourVel[i].y<<" "<<global.wm.ourVel[i].z<<endl;
	}
	for(i=0;i<11;i++){
		cout<<"P 2 "<<i+1<<" "<<global.wm.oppPos[i].x<<" "<<global.wm.oppPos[i].y<<" "<<global.wm.oppPos[i].z<<endl;
		cout<<"V 2 "<<i+1<<" "<<global.wm.oppVel[i].x<<" "<<global.wm.oppVel[i].y<<" "<<global.wm.oppVel[i].z<<endl;
	}
	cout<<"B "<<global.wm.ballPos.x<<" "<<global.wm.ballPos.y<<" "<<global.wm.ballPos.z<<" "<<global.wm.ballVel.x<<" "<<global.wm.ballVel.y<<" "<<global.wm.ballVel.z<<endl;

	// log the cycles with no flag/player/ball seen
	cout << "noseen";
	for (int i = 0; i < 8; ++i) {
		cout << " " << global.wm.noflag[i];
	}
	for (int i = 0; i < 11; ++i) {
		cout << " " << global.wm.noour[i];
	}
	for (int i = 0; i < 11; ++i) {
		cout << " " << global.wm.noopp[i];
	}
	cout << " " << global.wm.noball << endl;

//	cout<<"nAction:"<<global.nAction<<endl;
//	cout<<"nCommand:"<<global.nCommand<<endl;
  
  
//  cout<<global.wm.currentTime<<" "<<global.wm.myPos.x<<endl;
//  cout<<global.wm.currentTime<<" "<<global.wm.ballPos.x<<endl;
  	
}

#endif