sensor.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 "Sensor.h"
#include "Global_main.h"
#include "Global.h"

double Sensor::pi = acos(-1.);

Sensor::Sensor()
{
	bShouldILeave = false;
}

Sensor::~Sensor()
{
}

double Sensor::G(int n, double ff[])
{
    const double sigmaDi = 0.0965, sigmaTh = 0.1225, sigmaPhi = 0.1480;
    double x = ff[0], y = ff[1], z = ff[2], res = 0;
    for (int i = 0; i < 8; ++ i) {
        double dx = x - vFlag[i][0], dy = y - vFlag[i][1], dz = z - vFlag[i][2];
        double d = dx * dx + dy * dy;
        double dd = sqrt(d), di = sqrt(d + dz * dz);

		// distance
        res += (di - data[i].distance) * (di - data[i].distance) / 2. / sigmaDi / sigmaDi;

		// theta
        double th = acos((vFlag[i][0] - x) / dd);

		if (mTeamIndex == TI_RIGHT) {
			th = pi - th;
		}
		if (mTeamIndex == TI_LEFT && vFlag[i][1] - y < 0 || mTeamIndex == TI_RIGHT && y - vFlag[i][1] < 0) {
			th = -th;
		}
		th = th * 180. / pi;
        double deltaTh = fabs(th - data[i].theta);
        if (deltaTh > 180) {
            deltaTh = 360 - deltaTh;
        }
        res += deltaTh * deltaTh / 2. / sigmaTh / sigmaTh;
    
		// phi
        double phi = asin((vFlag[i][2] - z) / di);
        phi = phi * 180. / pi;
        double deltaPhi = fabs(phi - data[i].phi);
        res += deltaPhi * deltaPhi / 2. / sigmaPhi / sigmaPhi;
    }

    return res / 1000;
}

bool Sensor::NonConstraint(int n, double ff[])
{
    const int MAX_ITERATION = 20;
    const double delta = 1e-5, eps = 5e-3;
	int iter;
	double step, *p, *p2, base, len, len2, *ff2, *ff3, st;
    p = new double[n]; p2 = new double[n]; ff2 = new double[n]; ff3 = new double[n];

	for (int it = 0; it < MAX_ITERATION; ++ it) {
		base = G(n, ff);
		len = 0;
		for (int i = 0; i < n; ++ i) {
			ff[i] += delta;
			p[i] = -(G(n, ff) - base) / delta;
			ff[i] -= delta;
			len += p[i] * p[i];
		}
		len = sqrt(len);
        if (len < eps) {
            delete[] p; delete[] p2; delete[] ff2; delete[] ff3;
			return true;
        }
		for (iter = 0; iter < n - 1; ++ iter) {
			for (int i = 0; i < n; ++ i) {
				ff3[i] = ff2[i] = ff[i];
			}
			double ss[3];
            bool fl = true;
			for (step = 1 / len; step * len > delta; step *= 0.5, fl = false) {
			    for (int i = 0; i < n; ++ i) ff2[i] = ff3[i] - step * p[i];
			    ss[0] = G(n, ff2);
    		    for (int i = 0; i < n; ++ i) ff2[i] = ff3[i] + step * p[i];
				ss[2] = G(n, ff2);
                if (fl) {
				    for (int i = 0; i < n; ++ i) ff2[i] = ff3[i];
				    ss[1] = G(n, ff2);
                }
                if (ss[0] < ss[1] && ss[0] < ss[2]) {
					for (int i = 0; i < n; ++ i) ff3[i] -= step * p[i];
                    ss[1] = ss[0];
                } else if (ss[2] < ss[1] && ss[2] < ss[0]) {
					for (int i = 0; i < n; ++ i) ff3[i] += step * p[i];
                    ss[1] = ss[2];
                }
			}
            for (int i = 0; i < n; ++ i) {
                ff2[i] = ff3[i];
            }
			len2 = 0;
			base = G(n, ff2);
			for (int i = 0; i < n; ++ i) {
				ff2[i] += delta;
				p2[i] = -(G(n, ff2) - base) / delta;
				ff2[i] -= delta;
				len2 += p2[i] * p2[i];
			}
			len2 = sqrt(len2);
			if (len2 < eps) {
				for (int i = 0; i < n; ++ i)
					ff[i] = ff2[i];
                delete[] p; delete[] p2; delete[] ff2; delete[] ff3;
				return true;
			}
			st = len2 * len2 / (len * len);
			for (int i = 0; i < n; ++ i) {
				p[i] = p2[i] + st * p[i];
				ff[i] = ff2[i];
			}
			len = len2;
		}
	}

    delete[] p; delete[] p2; delete[] ff2; delete[] ff3;
	return false;
}

#endif