filter.cpp

浙江大学 RoboCup3D 2006 源代码

/***************************************************************************
 *   Copyright (C) 2004 - 2006 by ZJUBase                                  *
 *                                National Lab of Industrial Control Tech. *
 *                                Zhejiang University, China               *
 *                                                                         *
 *   Achievements of ZJUBase in RoboCup Soccer 3D Simulation League:       *
 *    In RoboCup championship,                                             *
 *      - June 2006 - 3rd place in RoboCup 2006, Bremen, Germany (lost     *
 *        the semi-final with a coin toss)                                 *
 *      - July 2005 - 3rd place in RoboCup 2005, Osaka, Japan (share the   *
 *        3rd place with team Caspian)                                     *
 *    In RoboCup local events,                                             *
 *      - April 2006 - 2nd place in RoboCup Iran Open 2006, Tehran, Iran   *
 *        (lost the final with a coin toss)                                *
 *      - December 2005 - 2nd place in AI Games 2005, Isfahan, Iran        *
 *      - July 2005 - champion in China Robot Competition 2005, Changzhou, *
 *        China                                                            *
 *      - October 2004 - champion in China Soccer Robot Competition 2004,  *
 *        Guangzhou, 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.             *
 ***************************************************************************/

#ifndef WIN32

#include "Global.h"
#include "Filter.h"
#include "Utils.h"
#include "Predictor.h"
#include "ServerSettings.h"
#include <iostream>

WMFilter filter;

BallFilter::BallFilter():effectBallCount(0)
{
	int i,j;	
	k_inair=1.0/0.8745;
	k_onground=1.0/0.9087;
	k = k_onground;
//   k=1.0/0.9035;

	for(i=0;i<POINT_MAX;i++)
		K[i] = getSumGeomSeries(1.0,k,i);

	for(i=0;i<POINT_MAX;i++){
		A[i] = i+1;
		B[i] = 0;
		for(j=0;j<i+1;j++)
			B[i] += getSumGeomSeries(1.0,k,j);
		C[i] = 0;
		for(j=0;j<i+1;j++)
			C[i] += getSumGeomSeries(1.0,k,j)*getSumGeomSeries(1.0,k,j);
	}

	IsBinair=0;
	ball_filter_count=1;
}

BallFilter::~BallFilter(){
}

bool BallFilter::IsBallinair(){
	if(fabs(global_previous[4].wm.ballPos.z-global.wm.ballPos.z)>0.35||global.wm.ballPos.z>0.11+0.14||fabs(global_previous[2].wm.ballPos.z-global.wm.ballPos.z)>0.25)
		return 1;
	else
		return 0;
}

void BallFilter::resetBallFilter(){
	int i,j;	

	k =(IsBinair) ? k_inair : k_onground;
	for(i=0;i<POINT_MAX;i++)
		K[i] = getSumGeomSeries(1.0,k,i);
	
	for(i=0;i<POINT_MAX;i++){
		A[i] = i+1;
		B[i] = 0;
		for(j=0;j<i+1;j++)
			B[i] += getSumGeomSeries(1.0,k,j);
		C[i] = 0;
		for(j=0;j<i+1;j++)
			C[i] += getSumGeomSeries(1.0,k,j)*getSumGeomSeries(1.0,k,j);
	}
	
	ball_filter_count =2;
}

void BallFilter::getBallDecay()
{
	double CurrentVel,LastVel,TemVel;
	CurrentVel = global.wm.ballPos.x -global_previous[1].wm.ballPos.x;
	LastVel = global_previous[1].wm.ballPos.x - global_previous[2].wm.ballPos.x;
	TemVel = CurrentVel/LastVel; 	

	if(TemVel < 1 && TemVel > 0.82)
		ss.B_decay = TemVel;
	else if(IsBallinair() == 1) 
		ss.B_decay = 0.8745;
	else
		ss.B_decay = 0.9087;
}

void BallFilter::filter(){
	int exIsBinair=IsBinair;
	if(IsBinair!=IsBallinair()){
		IsBinair=1-IsBinair;  //IsBallinair();
		resetBallFilter();
	}

	int i;
	double x,y,z,vx,vy,vz;
	double X,Y,Z,XX,YY,ZZ;
	Vector3 ball_pos[POINT_MAX];
	for(i=0;i<POINT_MAX;i++){
		ball_pos[i] = global_previous[i].wm.ballPos;
	} 

	int n;
	ball_filter_count ++;
	if(ball_filter_count>=POINT_MAX)
		ball_filter_count = POINT_MAX - 1;
	n = ball_filter_count;
	X = 0;

	for(i=0;i<=n;i++){
		X += ball_pos[i].x;
	}
	Y = 0;
	for(i=0;i<=n;i++){
		Y += ball_pos[i].y;
	}
	
	XX = 0;
	for(i=1;i<=n;i++){
		XX += ball_pos[i].x*K[i];
	}
	YY = 0;
	for(i=1;i<=n;i++){
		YY += ball_pos[i].y*K[i];
	}

	vx = (B[n]*X - A[n] * XX)/(B[n]*B[n] - A[n]*C[n]);
	vy = (B[n]*Y - A[n] * YY)/(B[n]*B[n] - A[n]*C[n]);
	x = (X - B[n]*vx)/A[n];
	y = (Y - B[n]*vy)/A[n];
	double error;
	error = 0;
	
	for(i=0;i<=n;i++){
		error += (x+K[i]*vx-ball_pos[i].x) *(x+K[i]*vx-ball_pos[i].x);
		error += (y+K[i]*vy-ball_pos[i].y) *(y+K[i]*vy-ball_pos[i].y);
	}

	vx = -vx;
	vy = -vy;

	if(error > 0.1){
		ball_filter_count = 2;
		global.wm.ballVel.x = (ball_pos[0] - ball_pos[1]).x/k;
		global.wm.ballVel.y = (ball_pos[0] - ball_pos[1]).y/k;
		if(fabs(ball_pos[0].z-ball_pos[1].z)>0.3&&ball_pos[0].z>0.2) {
			global.wm.ballVel.z = ((ball_pos[0].z- ball_pos[1].z)+14.6795*0.2)*0.8745 - 14.6795*0.2;
		}
		return;
		
	} else {
		if(IsBinair==0) {
			z=0.111;
		} else {
			z=global.wm.ballPos.z;
		}

		if(ball_filter_count>3)
			global.wm.ballPos = Vector3(x,y,z);

		global.wm.ballVel = Vector3(vx/k,vy/k,0);
	}

	if(IsBinair==0&&exIsBinair==0) {
		global.wm.ballVel.z=0;
	} else {
		if(fabs(ball_pos[0].z-ball_pos[1].z)>0.3&&ball_pos[0].z>0.2) {
			global.wm.ballVel.z = ((ball_pos[0].z- ball_pos[1].z)+14.6795*0.2)*0.8745 - 14.6795*0.2;
		} else {
			global.wm.ballVel.z=0;
		}
	}
	
	if(global.wm.ballVel.mod() > 5)
		global.wm.ballVel = 0;
}

MyPosFilter::MyPosFilter()
{
	k = 0.01616;
	T = 0.2;
	alpha = 2.5335;
	quo = exp(-alpha*T);

	double a = -(1-quo)/(alpha*quo);
	double b = -(k*T - (1-quo)/alpha*(k*(1-quo)/quo+k));
	double c = 1/quo;
	double d = -k*(1-quo)/quo;

	int i,j;
	B[0] = 0;
	B[1] = b;
	A[0] = 0;
	A[1] = a;

	for(i=1;i<POINT_MAX-1;i++){
		B[i+1] = B[i] + a*d*pow(c,i-1);
		A[i+1] = A[i] + a*pow(c,i);
	}

	for(i=0;i<POINT_MAX;i++){
		AA[i] = 0;
		BB[i] = 0;
		CC[i] = 0;
		for(j=1;j<=i;j++){
			AA[i] = i+1;
			BB[i] += A[j];
			CC[i] += A[j]*A[j];
		}
	}
	
	D[0] = 1;
	for(i=1;i<POINT_MAX;i++){
		D[i] = AA[i]*CC[i] - BB[i]*BB[i];
	}
}

MyPosFilter::~MyPosFilter()
{
}

void MyPosFilter::filter()
{
	Vector3 self_pos[POINT_MAX];
	Vector3 F[POINT_MAX];
	int i,j;
	int n;
	
	self_filter_count ++;
	if(self_filter_count>=7)
		self_filter_count= 7;
	n = self_filter_count;

	double Fx,Fy,FFx[POINT_MAX],FFy[POINT_MAX],FFFx,FFFy;
	for(i=0;i<POINT_MAX;i++){
		self_pos[i] = global_previous[i].wm.myPos;
	}
	for(i=0;i<POINT_MAX-2;i++){
		F[i] = global_previous[i+1].driveF;
	}
	for(i=0;i<=n;i++){
		FFx[i] = 0;
		FFy[i] = 0;
		for(j=0;j<i;j++){
			FFx[i] += F[j].x*B[i-j];
			FFy[i] += F[j].y*B[i-j];
		}
	}

	Fx = self_pos[0].x;
	Fy = self_pos[0].y;
	FFFx = 0;
	FFFy = 0;
	for(i=1;i<=n;i++){
		Fx += (self_pos[i].x - FFx[i]);
		FFFx += A[i]*(self_pos[i].x - FFx[i]);
		Fy += (self_pos[i].y - FFy[i]);
		FFFy += A[i]*(self_pos[i].y - FFy[i]);
	}

	double posx,posy,velx,vely;
	posx = (Fx*CC[n] - FFFx*BB[n])/D[n];
	posy = (Fy*CC[n] - FFFy*BB[n])/D[n];
	velx = (AA[n]*FFFx - Fx*BB[n])/D[n];
	vely = (AA[n]*FFFy - Fy*BB[n])/D[n];
	double err;
	err = 0;
	Vector3 pos;
	for(i=0;i<n;i++){
		pos.x = posx + A[i]*velx + FFx[i];
		pos.y = posy + A[i]*vely + FFy[i];
		double error = (self_pos[i].x - pos.x)*(self_pos[i].x - pos.x)+(self_pos[i].y - pos.y)*(self_pos[i].y - pos.y);
		err += error;
	}
	err/=n;
	
	double distFromOtherPlayer=1;
	for(i=0;i<11;i++){
		if(distFromOtherPlayer>(global.wm.myPos-global.wm.ourPos[i]).mod()&&i!=global.wm.myNumber-1)
			distFromOtherPlayer=(global.wm.myPos-global.wm.ourPos[i]).mod();
		if(distFromOtherPlayer>(global.wm.myPos-global.wm.oppPos[i]).mod())
			distFromOtherPlayer=(global.wm.myPos-global.wm.oppPos[i]).mod();
	}
	global.wm.myVel = Vector3(velx,vely,0);

	if(err > 0.1){
		global.wm.myPos = Vector3(global.wm.nextPos.x,global.wm.nextPos.y,0.22);
		self_filter_count = 1;
		return;
	}

	if(err > 0.01 && distFromOtherPlayer < 0.5){
		self_filter_count = 1;
		return;
	}

	if(err > 0.008){
		global.wm.myPos = Vector3(global.wm.nextPos.x,global.wm.nextPos.y,0.22);
		next_count++;
		if(next_count>4) {
			self_filter_count = 1;
			next_count = 0;
		}
		return;
	}

	next_count = 0;
	if(self_filter_count>=3){
		global.wm.myPos = Vector3(posx,posy,0.22);
	}
}

WMFilter::WMFilter()
{
}

WMFilter::~WMFilter()
{
}

void WMFilter::filterOfAgent()
{
	filterAgent.filter();
}

void WMFilter::filterOfBall()
{
	filterBall.filter();
}
	
#endif