utils.h

浙江大学 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 UTILS_H
#define UTILS_H

#include <cmath>
#include <iostream>
using namespace std;

#define V2V(v) Vector3((v).x, (v).y, (v).z)
#ifndef min
#define min(a, b) ((a) < (b) ? (a) : (b))
#endif
#ifndef max
#define max(a, b) ((a) > (b) ? (a) : (b))
#endif

const double PI = acos(-1.);
const double EPS = 1e-8;

class ServerSettings;
class Situation;

/*! CoordSystem is an enumeration of the different specified coordinate systems.
    The two possibilities are CARTESIAN or POLAR. These values are for instance
    used in the initializing a VecPosition. The CoordSystem indicates whether
    the supplied arguments represent the position in cartesian or in polar 
    coordinates. */
enum CoordSystemT {
  CARTESIAN,
  POLAR
};

typedef double AngRad;  /*!< Type definition for angles in degrees. */

// auxiliary numeric functions for determining the
// maximum and minimum of two given double values and the sign of a value
// various goniometric functions
typedef double Angle;
bool   isAngInInterval     ( Angle ang,    Angle angMin,    Angle angMax );
Angle getBisectorTwoAngles( Angle angMin, Angle angMax );

double cosDeg  ( Angle x             );
double sinDeg  ( Angle x             );
double tanDeg  ( Angle x             );
Angle atanDeg ( double x             );
double atan2Deg( double x,  double y  );
Angle acosDeg ( double x             );
Angle asinDeg ( double x             );
Angle Rad2Deg ( double x             );
double Deg2Rad ( Angle x             );

/******************************************************************************/
/********************   CLASS VECPOSITION   ***********************************/
/******************************************************************************/

/*! This class contains an x-, y- and z-coordinate of a position (x,y,x) as member
    data and methods which operate on this position. The standard arithmetic
      operators are overloaded and can thus be applied to positions (x,y,z). It is
      also possible to represent a position in polar coordinates (r,phi), since
      the class contains a method to convert these into cartesian coordinates
      (x,y,z). */
class Vector3 {
public:
	double x;
	double y;
	double z;
	double& operator [](int inx) { return inx ? (inx > 1 ? z : y) : x; }
	
	Vector3(double vx = 0, double vy = 0, double vz = 0) {
		x = vx; y = vy; z = vz;
	}

	Vector3(double r, double phi, double vz, CoordSystemT cs) {
		if (cs == POLAR) {
			x = r * cosDeg(phi); y = r * sinDeg(phi); z = vz;
		} else {
			x = r; y = phi; z = vz;
		}
	}

	double mod() {
		return sqrt(x*x + y*y + z*z);
	}

	double ang() {
		double mod = x*x + y*y;
		if(mod > 0){
			double r = sqrt(mod);
			return ( y>=0 ?  acos(x/r) : - acos(x/r)) * 180/PI;
		}
		return 0;
	}

	Vector3 setMagnitude(double d) {
		normalize();
		x *= d; y *= d; z *= d;
		return *this;
	}

	Vector3 normalize() {
		double m = mod();
		if (m < EPS) {
			x = y = z = 0;
		} else {
			x /= m; y /= m; z /= m;
		}
		return *this;
	}

	Angle getDirection() const {
		return atan2Deg(y, x);
	}

	Vector3 rotate2d(Angle angle) {
		double dMag    = this->mod();
		double dNewDir = this->getDirection() + angle;			// add rotation angle to phi
		x = dMag * cosDeg(dNewDir); y = dMag * sinDeg(dNewDir); // convert back to Cartesian
		return *this;
	}

	// calculate the cross product (p1-p0) * (p2-p0)
	static Vector3 vecCross(const Vector3& p1, const Vector3& p2) {
		return Vector3(
			p1.y * p2.z - p2.y * p1.z,
			p1.z * p2.x - p2.z * p1.x,
			p1.x * p2.y - p2.x * p1.y
		);
	}
	static double vecCross2d(const Vector3& p0, const Vector3& p1, const Vector3& p2) {
		return (p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y);
	}

	double getDistToSeg2d(Vector3& point1, Vector3& point2) {
		if ((point1 - point2).mod() < EPS) {
			return (*this - point1).mod();
		}

		Vector3 t = *this;
		t.x += point1.y - point2.y; t.y += point2.x - point1.x;
		if (vecCross2d(point1, t, *this) * vecCross2d(point2, t, *this) > EPS)
			return min((*this - point1).mod(), (*this - point2).mod());
		return fabs(vecCross2d(*this, point1, point2)) / (point1 - point2).mod();
	}

	~Vector3(){};
	// operators
	Vector3 operator -() { return Vector3(-x, -y, -z); }
	Vector3 operator +(const Vector3 &a) { return Vector3(x + a.x, y + a.y, z + a.z); }
	Vector3 operator -(const Vector3 &a) { return Vector3(x - a.x, y - a.y, z - a.z); }
	Vector3 operator *(const double &a) { return Vector3(x * a, y * a, z * a); }
	Vector3 operator /(const double &a) { return Vector3(x / a, y / a,z/a); }
	Vector3& operator =(const double &a) { x = a; y = a; z=z; return *this;}
	Vector3& operator +=(const Vector3 &a) { x += a.x;  y += a.y; z +=a.z; return *this;}
	Vector3& operator +=(const double &a) { x += a;  y += a; z +=a; return *this;}
	Vector3& operator -=(const Vector3 &a) { x -= a.x;  y -= a.y; z -=a.z; return *this;}
	Vector3& operator -=(const double &a) { x -= a;  y -= a; z -=a; return *this;}
	Vector3& operator *=(const double &a) { x *= a;  y *= a; z*=a; return *this;}