nonlinear.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.             *
 ***************************************************************************/

#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <string>
#pragma warning (disable : 4554)
using namespace std;

#define for if(0);else for
#include "Nonlinear.h"

const double pi = acos(-1.);

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

	AngleMax = 10. * pi / 180.;
	A = quo;
	B = k * (1 - quo);
	C = (1 - quo) / alpha;
	D = k * T - k * (1 - quo) / alpha;

	AA = B / (D * B + B * B * C - A * B * D);
	BB = (A * A * D - B * C - A * B * C) / (D * B + B * B * C - A * B * D);
	CC = (-A * B) / (D * B + B * B * C - A * B * D);
	DD = (A * B * C + A * A * B * C - A * A * D - A * A * B * C) / (D * B + B * B * C - A * B * D);
	RR = 0.005;
	R0 = 0.331;

	aa[0] = B; bb[0] = D;	
	double j = B * C;
	for (int i = 1; i < MAX_N; ++ i) {
		aa[i] = aa[i - 1] * A;
		bb[i] = bb[i - 1] + j;
		j *= A;
	}

	for (int i = 0; i < MAX_N; ++ i) {
		FF[i] = 100.;
	}
}

Nonlinear::~Nonlinear()
{
}

double Nonlinear::F(int n, double ff[], double x0, double y0, double v0x, double v0y, bool isJudge)
{
	double res = 0, temp;
	double x[MAX_N], y[MAX_N], vx[MAX_N], vy[MAX_N];
	x[0] = x0; y[0] = y0;
	vx[0] = v0x; vy[0] = v0y;

	for (int nn = n + 2; nn > 0; -- nn) {
		double aa0 = aa[nn - 1] * A / B;
		double bb0 = (bb[nn - 1] - D) / B * A + C;
		x[nn] = x0 + bb0 * v0x; y[nn] = y0 + bb0 * v0y;
		vx[nn] = aa0 * v0x; vy[nn] = aa0 * v0y;
		for (int i = 0; i < nn; ++ i) {
			x[nn] += bb[nn - 1 - i] * ff[i << 1];
			y[nn] += bb[nn - 1 - i] * ff[(i << 1) + 1];
			vx[nn] += aa[nn - 1 - i] * ff[i << 1];
			vy[nn] += aa[nn - 1 - i] * ff[(i << 1) + 1];
		}
	}

	temp = x[n + 2] + R0;
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;

	temp = -x[n + 2] - R0;
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	temp = y[n + 2];
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	temp = -y[n + 2];
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	temp = vx[n + 2];
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	temp = -vx[n + 2];
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	temp = vy[n + 2];
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	temp = -vy[n + 2];
	if (isJudge && temp < -eps)
		return eps * 10;
	temp = temp < 0 ? temp : 0;
	res += temp * temp;
	
	for (int i = 0; i < n + 2; ++ i) {
		temp = FF[n + 1 - i] * FF[n + 1 - i] - ff[i << 1] * ff[i << 1] - ff[(i << 1) + 1] * ff[(i << 1) + 1];
		if (isJudge && temp < -eps)
			return eps * 10;
		temp = temp < 0 ? temp : 0;
		res += temp * temp;
	}
	
	for (int i = 0; i <= n + 2; ++ i) {
		temp = x[i] * x[i] + y[i] * y[i] - R0 * R0;
		if (isJudge && temp < -eps)
			return eps * 10;
		temp = temp < 0 ? temp : 0;
		res += temp * temp;
	}
	
	double di = sqrt(y[n + 1] * y[n + 1] + (x[n + 1] + R0) * (x[n + 1] + R0));
	if (di > eps) {
		double angle = asin(y[n + 1] / di);
		temp = AngleMax * AngleMax - angle * angle;
		if (isJudge && temp < -eps)
			return eps * 10;
		temp = temp < 0 ? temp : 0;
		res += temp * temp;
	}
	
	return res;
}

bool Nonlinear::IsFeasible(int n, double ff[], double x0, double y0, double v0x, double v0y)
{
	double res = F(n, ff, x0, y0, v0x, v0y, true);
	return res < eps && res > -eps;
}

double Nonlinear::G(int n, double ff[], double x0, double y0, double v0x, double v0y, double r)
{
	double res = F(n, ff, x0, y0, v0x, v0y), temp = 0, tt;
	for (int i = 0; i < n + 2; ++ i) {
		if (temp > (tt = ff[i << 1] * ff[i << 1] + ff[(i << 1) + 1] * ff[(i << 1) + 1]))
			temp = tt;
	}

	res = temp + res / r;
	return res;
}

bool Nonlinear::NonConstraint(int n, double ff[], double x0, double y0, double v0x, double v0y, double r)
{
	int iter;
	double step, p[MAX_N << 1], p2[MAX_N << 1], base, len, len2, ff2[MAX_N << 1], st;

	for (int it = 0; it < MAX_ITERATION2; ++ it) {
		base = G(n, ff, x0, y0, v0x, v0y, r);
		len = 0;
	
		for (int i = 0; i < (n + 2 << 1); ++ i) {
			ff[i] += delta;
			p[i] = -(G(n, ff, x0, y0, v0x, v0y, r) - base) / delta;
			ff[i] -= delta;
			len += p[i] * p[i];
		}

		len = sqrt(len);
		if (len < eps)
			return true;
		
		for (iter = 0; iter < (n + 2 << 1) - 1; ++ iter) {
			for (int i = 0; i < (n + 2 << 1); ++ i) {
				ff2[i] = ff[i];
			}
		
			double ss[3];
			for (step = 1 / len; step * len > delta; step *= 0.5) {
				for (int i = 0; i < (n + 2 << 1); ++ i) ff2[i] -= step * p[i];
				ss[0] = G(n, ff2, x0, y0, v0x, v0y, r);
				for (int i = 0; i < (n + 2 << 1); ++ i) ff2[i] += step * 2 * p[i];
				ss[2] = G(n, ff2, x0, y0, v0x, v0y, r);
				for (int i = 0; i < (n + 2 << 1); ++ i) ff2[i] -= step * p[i];
				ss[1] = G(n, ff2, x0, y0, v0x, v0y, r);
				if (ss[0] < ss[1] && ss[0] < ss[2])
					for (int i = 0; i < (n + 2 << 1); ++ i) ff2[i] -= step * p[i];
				else if (ss[2] < ss[1] && ss[2] < ss[0])
					for (int i = 0; i < (n + 2 << 1); ++ i) ff2[i] += step * p[i];
			}
			
			len2 = 0;
			base = G(n, ff2, x0, y0, v0x, v0y, r);
			for (int i = 0; i < (n + 2 << 1); ++ i) {
				ff2[i] += delta;
				p2[i] = -(G(n, ff2, x0, y0, v0x, v0y, r) - base) / delta;
				ff2[i] -= delta;
				len2 += p2[i] * p2[i];
			}
			
			len2 = sqrt(len2);
			if (len2 < eps) {
				for (int i = 0; i < (n + 2 << 1); ++ i)
					ff[i] = ff2[i];
				return true;
			}
			
			st = len2 * len2 / (len * len);
			for (int i = 0; i < (n + 2 << 1); ++ i) {
				p[i] = p2[i] + st * p[i];
				ff[i] = ff2[i];
			}
			len = len2;
		}
	}

	return true;
}

bool Nonlinear::Solve(int n, double ff[], double x0, double y0, double v0x, double v0y)
{
	double last[MAX_N << 1];
	for (int i = 0; i < n + 2; ++ i) {
		last[i << 1] = ff[i << 1] + 1;
		last[(i << 1) + 1] = ff[(i << 1) + 1] + 1;
	}

	for (int i = 0; i < 5; ++ i) {
		FF[i] = 10. + i * 20.;
		FF[i] = FF[i] < 100. ? FF[i] : 100.;
	}
	
	int iter;
	double rr;
	for (iter = 0, rr = 10000; iter < MAX_ITERATION; ++ iter, rr *= 0.7) {
		if (this->NonConstraint(n, ff, x0, y0, v0x, v0y, rr) == false)
			return false;
		if (this->IsFeasible(n, ff, x0, y0, v0x, v0y))
			return true;

		bool end = true;
		for (int i = 0; end && i < n + 2; ++ i) {
			if (ff[i << 1] * ff[i << 1] + ff[(i << 1) + 1] * ff[(i << 1) + 1] < FF[n + 1 - i] * FF[n + 1 - i] - eps)
				end = false;
		}
		if (end) return false;
	}

	return false;
}

bool Nonlinear::Test(int n, double ff[], double x0, double y0, double v0x, double v0y)
{
	double x[MAX_N], y[MAX_N];
	for (int i = 0; i < n + 2; ++ i) {
		cout << "F" << i + 1 << " (" << ff[i << 1] << "," << ff[(i << 1) + 1] << ") ";
	}
	cout << endl;

	double vx = v0x, vy = v0y, xx, yy;
	x[0] = x0; y[0] = y0;
	cout << endl << "Test: " << endl;
	cout << "\tT = 0: (" << x[0] << "," << y[0] << "," << vx << "," << vy << ")" << endl;
	for (int i = 0; i < n + 2; ++ i) {
		xx = C * vx + D * ff[i << 1];
		yy = C * vy + D * ff[(i << 1) + 1];
		vx = A * vx + B * ff[i << 1];
		vy = A * vy + B * ff[(i << 1) + 1];
		x[i + 1] = x[i] + xx; y[i + 1] = y[i] + yy;
		cout << "\tT = " << i + 1 << ": (" << x[i + 1] << "," << y[i + 1] << "," << vx << "," << vy << ")" << endl;
	}
	cout << endl;

	ofstream out("temp.txt");
	for (int i = 0; i <= n + 2; ++ i)
		out << x[i] << " ";
	out << endl;
	for (int i = 0; i <= n + 2; ++ i)
		out << y[i] << " ";
	out << endl;
	
	return true;
}