geometry.cpp

一个Ｃ编写的足球机器人比赛程序

// Geometry.cpp: implementation of the Geometry class.
//
//////////////////////////////////////////////////////////////////////

//#include "stdafx.h"
#include "Geometry.h"
#include <stdio.h>    // needed for sprintf


/*! This function returns the sign of a give double.
1 is positive, -1 is negative
\param d1 first parameter
\return the sign of this double */
int sign( double d1 )
{
	return (d1>0)?1:-1;
}

/*! This function returns the maximum of two given doubles.
\param d1 first parameter
\param d2 second parameter
\return the maximum of these two parameters */
/*
double max( double d1, double d2 )
{
return (d1>d2)?d1:d2;
}
*/

/*! This function returns the minimum of two given doubles.
\param d1 first parameter
\param d2 second parameter
\return the minimum of these two parameters */
/*
double min( double d1, double d2 )
{
return (d1<d2)?d1:d2;
}
*/

/*! 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 */
AngDeg Rad2Deg( AngRad x )
{
	return ( x * 180 / M_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 */
AngRad Deg2Rad( AngDeg x )
{
	return ( x * M_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( AngDeg 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( AngDeg 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( AngDeg 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 */
AngDeg 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 ) < EPSILON && fabs( y ) < EPSILON )
		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 */
AngDeg 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 */
AngDeg asinDeg( double x )
{
	if( x >= 1 )
		return ( 90.0 );
	else if ( x <= -1 )
		return ( -90.0 );
	
	return ( Rad2Deg( asin( x ) ) );
}

/*! This function returns a boolean value which indicates whether the value
'ang' (from interval [-180..180] lies in the interval [angMin..angMax].
Examples: isAngInInterval( -100, 4, -150) returns false
isAngInInterval(   45, 4, -150) returns true
\param ang angle that should be checked
\param angMin minimum angle in interval
\param angMax maximum angle in interval
\return boolean indicating whether ang lies in [angMin..angMax] */
bool isAngInInterval( AngDeg ang, AngDeg angMin, AngDeg angMax )
{
	// convert all angles to interval 0..360
	if( ( ang    + 360 ) < 360 ) ang    += 360;
	if( ( angMin + 360 ) < 360 ) angMin += 360;
	if( ( angMax + 360 ) < 360 ) angMax += 360;
	
	if( angMin < angMax ) // 0 ---false-- angMin ---true-----angMax---false--360
		return angMin < ang && ang < angMax ;
	else                  // 0 ---true--- angMax ---false----angMin---true---360
		return !( angMax < ang && ang < angMin );
}

/*! This method returns the bisector (average) of two angles. It deals
with the boundary problem, thus when 'angMin' equals 170 and 'angMax'
equals -100, -145 is returned.
\param angMin minimum angle [-180,180]
\param angMax maximum angle [-180,180]
\return average of angMin and angMax. */
AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax )
{
	// separate sine and cosine part to circumvent boundary problem
	return VecPosition::normalizeAngle(
		atan2Deg( (sinDeg( angMin) + sinDeg( angMax ) )/2.0,
		(cosDeg( angMin) + cosDeg( angMax ) )/2.0 ) );
}

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

/*! Constructor for the VecPosition class. When the supplied
Coordinate System type equals CARTESIAN, the arguments x and y
denote the x- and y-coordinates of the new position. When it
equals POLAR however, the arguments x and y denote the polar
coordinates of the new position; in this case x is thus equal to
the distance r from the origin and y is equal to the angle phi
that the polar vector makes with the x-axis.
\param x the x-coordinate of the new position when cs == CARTESIAN; the
distance of the new position from the origin when cs = POLAR
\param y the y-coordinate of the new position when cs = CARTESIAN; the
angle that the polar vector makes with the x-axis when cs = POLAR
\param cs a CoordSystemT indicating whether x and y denote cartesian
coordinates or polar coordinates
\return the VecPosition corresponding to the given arguments */
VecPosition::VecPosition( double x, double y, CoordSystemT cs )
{
	setVecPosition( x, y, cs );
}

/*! Overloaded version of unary minus operator for VecPositions. It returns the
negative VecPosition, i.e. both the x- and y-coordinates are multiplied by
-1. The current VecPosition itself is left unchanged.
\return a negated version of the current VecPosition */
VecPosition VecPosition::operator - ( )
{
	return ( VecPosition( -m_x, -m_y ) );
}

/*! Overloaded version of the binary plus operator for adding a given double
value to a VecPosition. The double value is added to both the x- and
y-coordinates of the current VecPosition. The current VecPosition itself is
left unchanged.
\param d a double value which has to be added to both the x- and
y-coordinates of the current VecPosition
\return the result of adding the given double value to the current
VecPosition */
VecPosition VecPosition::operator + ( const double &d )
{
	return ( VecPosition( m_x + d, m_y + d ) );
}

/*! Overloaded version of the binary plus operator for VecPositions. It returns
the sum of the current VecPosition and the given VecPosition by adding their
x- and y-coordinates. The VecPositions themselves are left unchanged.
\param p a VecPosition
\return the sum of the current VecPosition and the given VecPosition */
VecPosition VecPosition::operator + ( const VecPosition &p )
{
	return ( VecPosition( m_x + p.m_x, m_y + p.m_y ) );
}

/*! Overloaded version of the binary minus operator for subtracting a
given double value from a VecPosition. The double value is
subtracted from both the x- and y-coordinates of the current
VecPosition. The current VecPosition itself is left unchanged.
\param d a double value which has to be subtracted from both the x- and
y-coordinates of the current VecPosition
\return the result of subtracting the given double value from the current
VecPosition */
VecPosition VecPosition::operator - ( const double &d )
{
	return ( VecPosition( m_x - d, m_y - d ) );
}

/*! Overloaded version of the binary minus operator for
VecPositions. It returns the difference between the current
VecPosition and the given VecPosition by subtracting their x- and
y-coordinates. The VecPositions themselves are left unchanged.

  \param p a VecPosition
  \return the difference between the current VecPosition and the given
VecPosition */
VecPosition VecPosition::operator - ( const VecPosition &p )
{
	return ( VecPosition( m_x - p.m_x, m_y - p.m_y ) );
}

/*! Overloaded version of the multiplication operator for multiplying a
VecPosition by a given double value. Both the x- and y-coordinates of the
current VecPosition are multiplied by this value. The current VecPosition
itself is left unchanged.
\param d the multiplication factor
\return the result of multiplying the current VecPosition by the given
double value */
VecPosition VecPosition::operator * ( const double &d  )
{
	return ( VecPosition( m_x * d, m_y * d  ) );
}

/*! Overloaded version of the multiplication operator for
VecPositions. It returns the product of the current VecPosition
and the given VecPosition by multiplying their x- and
y-coordinates. The VecPositions themselves are left unchanged.

  \param p a VecPosition
\return the product of the current VecPosition and the given VecPosition */
VecPosition VecPosition::operator * ( const VecPosition &p )
{
	return ( VecPosition( m_x * p.m_x, m_y * p.m_y ) );
}

/*! Overloaded version of the division operator for dividing a
VecPosition by a given double value. Both the x- and y-coordinates
of the current VecPosition are divided by this value. The current
VecPosition itself is left unchanged.

  \param d the division factor
  \return the result of dividing the current VecPosition by the given double
value */
VecPosition VecPosition::operator / ( const double &d )
{
	return ( VecPosition( m_x / d, m_y / d  ) );
}

/*! Overloaded version of the division operator for VecPositions. It
returns the quotient of the current VecPosition and the given
VecPosition by dividing their x- and y-coordinates. The
VecPositions themselves are left unchanged.

  \param p a VecPosition
\return the quotient of the current VecPosition and the given one */
VecPosition VecPosition::operator / ( const VecPosition &p )
{
	return ( VecPosition( m_x / p.m_x, m_y / p.m_y ) );
}

/*! Overloaded version of the assignment operator for assigning a given double
value to both the x- and y-coordinates of the current VecPosition. This
changes the current VecPosition itself.
\param d a double value which has to be assigned to both the x- and
y-coordinates of the current VecPosition */
void VecPosition::operator = ( const double &d )
{
	m_x = d;
	m_y = d;
}

/*! Overloaded version of the sum-assignment operator for VecPositions. It
returns the sum of the current VecPosition and the given VecPosition by
adding their x- and y-coordinates. This changes the current VecPosition
itself.
\param p a VecPosition which has to be added to the current VecPosition */
void VecPosition::operator +=( const VecPosition &p )
{
	m_x += p.m_x;
	m_y += p.m_y;
}

/*! Overloaded version of the sum-assignment operator for adding a given double
value to a VecPosition. The double value is added to both the x- and
y-coordinates of the current VecPosition. This changes the current
VecPosition itself.
\param d a double value which has to be added to both the x- and
y-coordinates of the current VecPosition */
void VecPosition::operator += ( const double &d )
{
	m_x += d;
	m_y += d;
}

/*! Overloaded version of the difference-assignment operator for
VecPositions.  It returns the difference between the current
VecPosition and the given VecPosition by subtracting their x- and
y-coordinates. This changes the current VecPosition itself.

  \param p a VecPosition which has to be subtracted from the current
VecPosition */
void VecPosition::operator -=( const VecPosition &p )
{
	m_x -= p.m_x;
	m_y -= p.m_y;
}

/*! Overloaded version of the difference-assignment operator for
subtracting a given double value from a VecPosition. The double
value is subtracted from both the x- and y-coordinates of the
current VecPosition. This changes the current VecPosition itself.

  \param d a double value which has to be subtracted from both the x- and
y-coordinates of the current VecPosition */
void VecPosition::operator -=( const double &d )
{
	m_x -= d;
	m_y -= d;
}

/*! Overloaded version of the multiplication-assignment operator for
VecPositions. It returns the product of the current VecPosition
and the given VecPosition by multiplying their x- and
y-coordinates. This changes the current VecPosition itself.

  \param p a VecPosition by which the current VecPosition has to be
multiplied */
void VecPosition::operator *=( const VecPosition &p )
{
	m_x *= p.m_x;
	m_y *= p.m_y;
}

/*! Overloaded version of the multiplication-assignment operator for
multiplying a VecPosition by a given double value. Both the x- and
y-coordinates of the current VecPosition are multiplied by this
value. This changes the current VecPosition itself.

  \param d a double value by which both the x- and y-coordinates of the
current VecPosition have to be multiplied */
void VecPosition::operator *=( const double &d )
{
	m_x *= d;
	m_y *= d;
}

/*! Overloaded version of the division-assignment operator for
VecPositions. It returns the quotient of the current VecPosition
and the given VecPosition by dividing their x- and
y-coordinates. This changes the current VecPosition itself.

\param p a VecPosition by which the current VecPosition is divided */
void VecPosition::operator /=( const VecPosition &p )