geometryr.cpp

robocup 5vs5修改板决策例程

{
	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 has to be divided */
void VecPosition::operator /=( const VecPosition &p )
{
	m_x /= p.m_x;
	m_y /= p.m_y;
}

/*! Overloaded version of the division-assignment 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. 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 divided */
void VecPosition::operator /=( const double &d )
{
	m_x /= d;
	m_y /= d;
}

/*! Overloaded version of the inequality operator for VecPositions. It
determines whether the current VecPosition is unequal to the given
VecPosition by comparing their x- and y-coordinates.
\param p a VecPosition
\return true when either the x- or y-coordinates of the given VecPosition
and the current VecPosition are different; false otherwise */
bool VecPosition::operator !=( const VecPosition &p )
{
	return ( ( fabs(m_x - p.m_x) > EPS ) || ( fabs(m_y - p.m_y) > EPS ) );
}

/*! Overloaded version of the inequality operator for comparing a VecPosition to
a double value. It determines whether either the x- or y-coordinate of the
current VecPosition is unequal to the given double value.
\param d a double value with which both the x- and y-coordinates of the
current VecPosition have to be compared.
\return true when either the x- or y-coordinate of the current VecPosition
is unequal to the given double value; false otherwise */
bool VecPosition::operator !=( const double &d )
{
	return ( ( fabs(m_x - d) > EPS ) || ( fabs(m_y - d) > EPS ) );
}

/*! Overloaded version of the equality operator for VecPositions. It determines
whether the current VecPosition is equal to the given VecPosition by
comparing their x- and y-coordinates.
\param p a VecPosition
\return true when both the x- and y-coordinates of the given VecPosition and
the current VecPosition are equal; false otherwise */
bool VecPosition::operator ==( const VecPosition &p )
{
	return ( ( fabs(m_x - p.m_x) < EPS ) && ( fabs(m_y - p.m_y) < EPS ) );
}

/*! Overloaded version of the equality operator for comparing a VecPosition to a
double value. It determines whether both the x- and y-coordinates of the
current VecPosition are equal to the given double value.
\param d a double value with which both the x- and y-coordinates of the
current VecPosition have to be compared.
\return true when both the x- and y-coordinates of the current VecPosition
are equal to the given double value; false otherwise */
bool VecPosition::operator ==( const double &d )
{
	return ( ( fabs(m_x - d) < EPS ) && ( fabs(m_y - d) < EPS ) );
}


/*! 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 VecPosition are printed in the
format (x,y).
\param os output stream to which information should be written
\param v a VecPosition which must be printed
\return output stream containing (x,y) */
/*
::ostream& operator <<( ::ostream &os, VecPosition v )
{
return ( os << "( " << v.m_x << ", " << v.m_y << " )" );
}
*/



/*! This method writes the current VecPosition to a string. It can also write a
polar representation of the current VecPosition.
\param cs a CoordSystemtT indicating whether a POLAR or CARTESIAN
representation of the current VecPosition should be written
\return a string containing a polar or Cartesian representation of the
current VecPosition depending on the value of the boolean argument */
void VecPosition::ToString( char buf[], CoordSystemT cs)
{
	if( cs == CARTESIAN )
	wsprintf( buf, "( %5.2f, %5.2f )", GetX( ), GetY( ) );
	else
	wsprintf( buf, "( r: %5.2f, phi: %5.2f )", GetMagnitude( ), GetDirection( ) );
}

/*! Set method for the x-coordinate of the current VecPosition.
\param dX a double value representing a new x-coordinate
\return a boolean indicating whether the Update was successful */
bool VecPosition::SetX( double dX )
{
	m_x = dX;
	return ( true );
}

/*! Get method for the x-coordinate of the current VecPosition.
\return the x-coordinate of the current VecPosition */
double VecPosition::GetX( ) const
{
	return ( m_x );
}

/*! Set method for the y-coordinate of the current VecPosition.
\param dY a double value representing a new y-coordinate
\return a boolean indicating whether the Update was successful */
bool VecPosition::SetY( double dY )
{
	m_y = dY;
	return ( true );
}

/*! Get method for the y-coordinate of the current VecPosition.
\return the y-coordinate of the current VecPosition */
double VecPosition::GetY( ) const
{
	return ( m_y );
}

/*! This method (re)Sets the coordinates of the current VecPosition. The given
coordinates can either be polar or Cartesian coordinates. This is indicated
by the value of the third argument.
\param dX a double value indicating either a new Cartesian x-coordinate when
cs=CARTESIAN or a new polar r-coordinate (distance) when cs=POLAR
\param dY a double value indicating either a new Cartesian y-coordinate when
cs=CARTESIAN or a new polar phi-coordinate (angle) when cs=POLAR
\param cs a CoordSystemT indicating whether x and y denote cartesian
coordinates or polar coordinates */
void VecPosition::SetVecPosition( double dX, double dY, CoordSystemT cs)
{
	if( cs == CARTESIAN )
	{
		m_x = dX;
		m_y = dY;
	}
	else
	*this = GetVecPositionFromPolar( dX, dY );
}

/*! This method determines the distance between the current VecPosition and a
given VecPosition. This is equal to the magnitude (length) of the vector
connecting the two positions which is the difference vector between them.
\param p a Vecposition
\return the distance between the current VecPosition and the given
VecPosition */
double VecPosition::GetDistanceTo( const VecPosition p )
{
	return ( ( *this - p ).GetMagnitude( ) );
}

/*! This method adjusts the coordinates of the current VecPosition in such a way
that the magnitude of the corresponding vector equals the double value which
is supplied as an argument. It thus scales the vector to a given length by
multiplying both the x- and y-coordinates by the quotient of the argument
and the current magnitude. This changes the VecPosition itself.
\param d a double value representing a new magnitude
\return the result of scaling the vector corresponding with the current
VecPosition to the given magnitude thus yielding a different VecPosition */
VecPosition VecPosition::SetMagnitude( double d )
{
	if( GetMagnitude( ) > EPS )
	( *this ) *= ( d / GetMagnitude( ) );

	return ( *this );
}

/*! This method determines the magnitude (length) of the vector corresponding
with the current VecPosition using the formula of Pythagoras.
\return the length of the vector corresponding with the current
VecPosition */
double VecPosition::GetMagnitude( ) const
{
	return ( sqrt( m_x * m_x + m_y * m_y ) );
}

/*! This method determines the direction of the vector corresponding with the
current VecPosition (the phi-coordinate in polar representation) using the
arc tangent function. Note that the signs of x and y have to be taken into
account in order to determine the correct quadrant.
\return the direction in degrees of the vector corresponding with the
current VecPosition */
AngRad VecPosition::GetDirection( ) const
{
	return ( atan2( m_y, m_x ) );
}

/*! This method determines whether the current VecPosition is in front of a
given VecPosition, i.e. whether the x-coordinate of the current VecPosition
is larger than the x-coordinate of the given VecPosition.
\param p a VecPosition to which the current VecPosition must be compared
\return true when the current VecPosition is in front of the given
VecPosition; false otherwise */
bool VecPosition::IsRightOf( const VecPosition &p )
{
	return ( ( m_x >= p.GetX( ) ) ? true : false );
}

/*! This method determines whether the x-coordinate of the current VecPosition
is in front of (i.e. larger than) a given double value.
\param d a double value to which the current x-coordinate must be compared
\return true when the current x-coordinate is in front of the given value;
false otherwise */
bool VecPosition::IsRightOf( const double &d )
{
	return ( ( m_x >= d ) ? true : false );
}

/*! This method determines whether the current VecPosition is behind a given
VecPosition, i.e. whether the x-coordinate of the current VecPosition is
smaller than the x-coordinate of the given VecPosition.
\param p a VecPosition to which the current VecPosition must be compared
\return true when the current VecPosition is behind the given VecPosition;
false otherwise */
bool VecPosition::IsLeftOf( const VecPosition &p )
{
	return ( ( m_x <= p.GetX( ) ) ? true : false );
}

/*! This method determines whether the x-coordinate of the current VecPosition
is behind (i.e. smaller than) a given double value.
\param d a double value to which the current x-coordinate must be compared
\return true when the current x-coordinate is behind the given value; false
otherwise */
bool VecPosition::IsLeftOf( const double &d )
{
	return ( ( m_x <= d ) ? true : false );
}

/*! This method determines whether the current VecPosition is to the left of a
given VecPosition, i.e. whether the y-coordinate of the current VecPosition
is smaller than the y-coordinate of the given VecPosition.
\param p a VecPosition to which the current VecPosition must be compared
\return true when the current VecPosition is to the left of the given
VecPosition; false otherwise */
bool VecPosition::IsButtomOf( const VecPosition &p )
{
	return ( ( m_y <= p.GetY( ) ) ? true : false );
}

/*! This method determines whether the y-coordinate of the current VecPosition
is to the left of (i.e. smaller than) a given double value.
\param d a double value to which the current y-coordinate must be compared
\return true when the current y-coordinate is to the left of the given
value; false otherwise */
bool VecPosition::IsButtomOf( const double &d )
{
	return ( ( m_y <= d ) ? true : false );
}

/*! This method determines whether the current VecPosition is to the right of a
given VecPosition, i.e. whether the y-coordinate of the current VecPosition
is larger than the y-coordinate of the given VecPosition.
\param p a VecPosition to which the current VecPosition must be compared
\return true when the current VecPosition is to the right of the given
VecPosition; false otherwise */
bool VecPosition::IsTopOf( const VecPosition &p )
{
	return ( ( m_y >= p.GetY( ) ) ? true : false );
}

/*! This method determines whether the y-coordinate of the current VecPosition
is to the right of (i.e. larger than) a given double value.
\param d a double value to which the current y-coordinate must be compared
\return true when the current y-coordinate is to the right of the given
value; false otherwise */
bool VecPosition::IsTopOf( const double &d )
{
	return ( ( m_y >= d ) ? true : false );
}

/*! This method determines whether the current VecPosition is in between two
given VecPositions when looking in the x-direction, i.e. whether the current
VecPosition is in front of the first argument and behind the second.
\param p1 a VecPosition to which the current VecPosition must be compared
\param p2 a VecPosition to which the current VecPosition must be compared
\return true when the current VecPosition is in between the two given
VecPositions when looking in the x-direction; false otherwise */
bool VecPosition::IsBetweenX( const VecPosition &p1, const VecPosition &p2 )
{
	return ( ( IsRightOf( p1 ) && IsLeftOf( p2 ) ) ? true : false );
}

/*! This method determines whether the x-coordinate of the current VecPosition
is in between two given double values, i.e. whether the x-coordinate of the
current VecPosition is in front of the first argument and behind the second.
\param d1 a double value to which the current x-coordinate must be compared
\param d2 a double value to which the current x-coordinate must be compared
\return true when the current x-coordinate is in between the two given
values; false otherwise */
bool VecPosition::IsBetweenX( const double &d1, const double &d2 )
{
	return ( ( IsRightOf( d1 ) && IsLeftOf( d2 ) ) ? true : false );
}

/*! This method determines whether the current VecPosition is in between two
given VecPositions when looking in the y-direction, i.e. whether the current
VecPosition is to the right of the first argument and to the left of the
second.
\param p1 a VecPosition to which the current VecPosition must be compared
\param p2 a VecPosition to which the current VecPosition must be compared
\return true when the current VecPosition is in between the two given
VecPositions when looking in the y-direction; false otherwise */
bool VecPosition::IsBetweenY( const VecPosition &p1, const VecPosition &p2 )
{
	return ( ( IsTopOf( p1 ) && IsButtomOf( p2 ) ) ? true : false );
}

/*! This method determines whether the y-coordinate of the current VecPosition
is in between two given double values, i.e. whether the y-coordinate of the
current VecPosition is to the right of the first argument and to the left
of the second.
\param d1 a double value to which the current y-coordinate must be compared
\param d2 a double value to which the current y-coordinate must be compared
\return true when the current y-coordinate is in between the two given
values; false otherwise */
bool VecPosition::IsBetweenY( const double &d1, const double &d2 )
{
	return ( ( IsTopOf( d1 ) && IsButtomOf( d2 ) ) ? true : false );
}

/*! This method Normalizes a VecPosition by Setting the magnitude of the
corresponding vector to 1. This thus changes the VecPosition itself.
\return the result of normalizing the current VecPosition thus yielding a
different VecPosition */
VecPosition VecPosition::Normalize( )
{
	return ( SetMagnitude( 1.0 ) );
}

/*! This method Rotates the vector corresponding to the current VecPosition over
a given angle thereby changing the current VecPosition itself. This is done
by calculating the polar coordinates of the current VecPosition and adding
the given angle to the phi-coordinate in the polar representation. The polar
coordinates are then converted back to Cartesian coordinates to obtain the
desired result.
\param angle an angle in degrees over which the vector corresponding to the
current VecPosition must be Rotated