📄 utils.cpp
字号:
/*************************************************************************** * 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 (jianghao@iipc.zju.edu.cn; riveria@gmail.com) *
* In the next season, the leader will be * * Yifeng ZHANG (yfzhang@iipc.zju.edu.cn) *
* ZJUBase 3D agent is created by * * Dijun LUO (djluo@iipc.zju.edu.cn) *
* All the members who has ever contributed: * * Jun JIANG *
* Xinfeng DU (xfdu@iipc.zju.edu.cn) *
* Yang ZHOU (yzhou@iipc.zju.edu.cn) *
* Zhipeng YANG *
* Xiang FAN *
* *
* Team Manager: *
* Ms. Rong XIONG (rxiong@iipc.zju.edu.cn) *
* *
* 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 "Global.h"
#include "Utils.h"
#include "ServerSettings.h"
#include "Situation.h"
#include "math.h"
#include <string>
using namespace std;
bool isContain(const char* whole,const char* sub)
{
char tmp[1024];
int len =(int)strlen(sub);
memcpy(tmp,whole,len);
tmp[len] = '\0';
return !strcmp(tmp,sub);
}
double getSumGeomSeries( double dFirst, double dRatio, double dLength)
{
// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
// subtract: s - sr = a - ar^n) => s = a(1-r^n)/(1-r)
if(dRatio == 1)
return 0;
return dFirst * ( 1 - pow( dRatio, dLength ) ) / ( 1 - dRatio ) ;
}
/*! This function converts an angle in radians to the corresponding angle in
* degrees.
* \param x an angle in radians
* \return the corresponding angle in degrees */
Angle Rad2Deg( double x )
{
return ( x * 180 / PI );
}
/*! This function converts an angle in degrees to the corresponding angle in
* radians.
* \param x an angle in degrees
* \return the corresponding angle in radians */
double Deg2Rad( Angle x )
{
return ( x * PI / 180 );
}
/*! This function returns the cosine of a given angle in degrees using the
* built-in cosine function that works with angles in radians.
* \param x an angle in degrees
* \return the cosine of the given angle */
double cosDeg( Angle x )
{
return ( cos( Deg2Rad( x ) ) );
}
/*! This function returns the sine of a given angle in degrees using the
* built-in sine function that works with angles in radians.
* \param x an angle in degrees
* \return the sine of the given angle */
double sinDeg( Angle x )
{
return ( sin( Deg2Rad( x ) ) );
}
/*! This function returns the tangent of a given angle in degrees using the
* built-in tangent function that works with angles in radians.
* \param x an angle in degrees
* \return the tangent of the given angle */
double tanDeg( Angle x )
{
return ( tan( Deg2Rad( x ) ) );
}
/*! This function returns the principal value of the arc tangent of x in degrees
* using the built-in arc tangent function which returns this value in radians.
* \param x a double value
* \return the arc tangent of the given value in degrees */
Angle atanDeg( double x )
{
return ( Rad2Deg( atan( x ) ) );
}
/*! This function returns the principal value of the arc tangent of y/x in
* degrees using the signs of both arguments to determine the quadrant of the
* return value. For this the built-in 'atan2' function is used which returns
* this value in radians.
* \param x a double value
* \param y a double value
* \return the arc tangent of y/x in degrees taking the signs of x and y into
* account */
double atan2Deg( double x, double y )
{
if( fabs( x ) < EPS && fabs( y ) < EPS )
return ( 0.0 );
return ( Rad2Deg( atan2( x, y ) ) );
}
/*! This function returns the principal value of the arc cosine of x in degrees
* using the built-in arc cosine function which returns this value in radians.
* \param x a double value
* \return the arc cosine of the given value in degrees */
Angle acosDeg( double x )
{
if( x >= 1 )
return ( 0.0 );
else if( x <= -1 )
return ( 180.0 );
return ( Rad2Deg( acos( x ) ) );
}
/*! This function returns the principal value of the arc sine of x in degrees
* using the built-in arc sine function which returns this value in radians.
* \param x a double value
* \return the arc sine of the given value in degrees */
Angle asinDeg( double x )
{
if( x >= 1 )
return ( 90.0 );
else if ( x <= -1 )
return ( -90.0 );
return ( Rad2Deg( asin( x ) ) );
}
Vector3 PolarV(Dist ro,Angle theta)
{
return Vector3(ro * cosDeg(theta),ro * sinDeg(theta),0);
}
double Sqr(double x)
{
return x*x;
}
int isInSector(Vector3 vecA, Vector3 vecB, double ang, double mod) //add by zy
{
Angle ang2 = fabs(vecA.ang()-vecB.ang());
if (ang2 > 180) ang2 = 360 - ang2;
if(ang2<ang && vecB.mod()<mod)
return 1;
else
return 0;
}
/*! Overloaded version of the C++ output operator for VecPositions. This
operator makes it possible to use VecPositions in output statements (e.g.
cout << v). The x- and y-coordinates of the Vector3 are printed in the
format (x,y).
\param os output stream to which information should be written
\param v a Vector3 which must be printed
\return output stream containing (x,y) */
ostream& operator <<( ostream &os, Vector3& v )
{
os << "( " << v.x << ", " << v.y << ", " << v.z << " )";
return os;
}
bool Vector3::isInField(double margine)
{
return (bool)(fabs(x) < Situation::SS->mFieldLength / 2 - margine
&& fabs(y) < Situation::SS->mFieldWidth / 2 - margine);
}
double Vector3::getDistToOppGoal()
{
Vector3 g1(X(100), -Situation::SS->mGoalWidth/2, z);
Vector3 g2(X(100), Situation::SS->mGoalWidth/2, z);
return getDistToSeg2d(g1, g2);
}
double Vector3::getDistToOurGoal()
{
Vector3 g1(-X(100), -Situation::SS->mGoalWidth/2, z);
Vector3 g2(-X(100), Situation::SS->mGoalWidth/2, z);
return getDistToSeg2d(g1, g2);
}
bool Vector3::isInOurForbiddenZone()
{
return x >= -X(100) && x <= -X(100) + Situation::SS->mPenaltyAreaSize
&& fabs(y) < Situation::SS->mGoalWidth/2 + Situation::SS->mPenaltyAreaSize;
}
bool Vector3::isInOppForbiddenZone()
{
return x <= X(100) && x >= X(100) - Situation::SS->mPenaltyAreaSize
&& fabs(y) < Situation::SS->mGoalWidth/2 + Situation::SS->mPenaltyAreaSize;
}
Angle Normalize(Angle ang)
{
Angle a = ang;
if(fabs(a)>1000){
return 0;
}
while(a>180)
a -= 360;
while(a<-180)
a += 360;
return a;
}
/******************************************************************************/
/*********************** CLASS GEOMETRY ***************************************/
/******************************************************************************/
/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the
length of a geometric series given its first element, the sum of the
elements in the series and the constant ratio between the elements.
Normally: s = a + ar + ar^2 + ... + ar^n
Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2 + .. + dFist*dRatio^n
\param dFirst first term of the series
\param dRatio ratio with which the the first term is multiplied
\param dSum the total sum of all the serie
\return the length(n in above example) of the series */
double Geometry::getLengthGeomSeries( double dFirst, double dRatio, double dSum )
{
if( dRatio < 0 )
cerr << "(Geometry:getLengthGeomSeries): negative ratio" << endl;
// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
// subtract: sr - s = - a + ar^n) => s(1-r)/a + 1 = r^n = temp
// log r^n / n = n log r / log r = n = length
double temp = (dSum * ( dRatio - 1 ) / dFirst) + 1;
if( temp <= 0 )
return -1.0;
return log( temp ) / log( dRatio ) ;
}
/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the sum of a
geometric series given its first element, the ratio and the number of steps
in the series
Normally: s = a + ar + ar^2 + ... + ar^n
Now: dSum = dFirst + dFirst*dRatio + ... + dFirst*dRatio^dSteps
\param dFirst first term of the series
\param dRatio ratio with which the the first term is multiplied
\param dSum the number of steps to be taken into account
\return the sum of the series */
double Geometry::getSumGeomSeries( double dFirst, double dRatio, double dLength)
{
// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
// subtract: s - sr = a - ar^n) => s = a(1-r^n)/(1-r)
return dFirst * ( 1 - pow( dRatio, dLength ) ) / ( 1 - dRatio ) ;
}
/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the sum of an
infinite geometric series given its first element and the constant ratio
between the elements. Note that such an infinite series will only converge
when 0<r<1.
Normally: s = a + ar + ar^2 + ar^3 + ....
Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2...
\param dFirst first term of the series
\param dRatio ratio with which the the first term is multiplied
\return the sum of the series */
double Geometry::getSumInfGeomSeries( double dFirst, double dRatio )
{
if( dRatio > 1 )
cerr << "(Geometry:CalcLengthGeomSeries): series does not converge" << endl;
// s = a(1-r^n)/(1-r) with n->inf and 0<r<1 => r^n = 0
return dFirst / ( 1 - dRatio );
}
/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the first element
of a geometric series given its element, the ratio and the number of steps
in the series
Normally: s = a + ar + ar^2 + ... + ar^n
Now: dSum = dFirst + dFirst*dRatio + ... + dFirst*dRatio^dSteps
\param dSum sum of the series
\param dRatio ratio with which the the first term is multiplied
\param dSum the number of steps to be taken into account
\return the first element (a) of a serie */
double Geometry::getFirstGeomSeries( double dSum, double dRatio, double dLength)
{
// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
// subtract: s - sr = a - ar^n) => s = a(1-r^n)/(1-r) => a = s*(1-r)/(1-r^n)
return dSum * ( 1 - dRatio )/( 1 - pow( dRatio, dLength ) ) ;
}
/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the first element
of an infinite geometric series given its first element and the constant
ratio between the elements. Note that such an infinite series will only
converge when 0<r<1.
Normally: s = a + ar + ar^2 + ar^3 + ....
Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2...
\param dSum sum of the series
\param dRatio ratio with which the the first term is multiplied
\return the first term of the series */
double Geometry::getFirstInfGeomSeries( double dSum, double dRatio )
{
if( dRatio > 1 )
cerr << "(Geometry:getFirstInfGeomSeries):series does not converge" << endl;
// s = a(1-r^n)/(1-r) with r->inf and 0<r<1 => r^n = 0 => a = s ( 1 - r)
return dSum * ( 1 - dRatio );
}
/*! This method performs the abc formula (Pythagoras' Theorem) on the given
parameters and puts the result in *s1 en *s2. It returns the number of
found coordinates.
\param a a parameter in abc formula
\param b b parameter in abc formula
\param c c parameter in abc formula
\param *s1 first result of abc formula
\param *s2 second result of abc formula
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -