geometryr.cpp

该程序实现FIRE足球机器人竞赛中的3：3比赛源码

	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 standard output. It can also
print a polar representation of the current VecPosition.
\param cs a CoordSystemtT indicating whether a POLAR or CARTESIAN
representation of the current VecPosition should be printed */
void VecPosition::Show( CoordSystemT cs )
{
	if( cs == CARTESIAN )
	printf("x = %5.2f, y = %5.2f", m_x, m_y);
	else
	printf("r = %5.2f, phi = %5.2f", GetMagnitude(), GetDirection( ));
}

void VecPosition::Print( FILE* file )
{
	fprintf(file, "%2.2f\t%2.2f", m_x, 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 )
	sprintf( buf, "( %5.2f, %5.2f )", GetX( ), GetY( ) );
	else
	sprintf( 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
\return the result of rotating the vector corresponding to the current
VecPosition over the given angle thus yielding a different VecPosition */
VecPosition VecPosition::Rotate( AngRad angle )
{
	// determine the polar representation of the current VecPosition
	double dMag    = this->GetMagnitude( );
	double dNewDir = this->GetDirection( ) + angle;  // add rotation angle to phi
	SetVecPosition( dMag, dNewDir, POLAR );          // convert back to Cartesian
	return ( *this );
}

/*! This method converts the coordinates of the current VecPosition (which are
represented in an global coordinate system with the origin at (0,0)) into
relative coordinates in a different coordinate system (e.g. relative to a
player). The new coordinate system is defined by the arguments to the
method. The relative coordinates are now obtained by aligning the relative
coordinate system with the global coordinate system using a translation to
Make both origins coincide followed by a rotation to align the axes.
\param origin the origin of the relative coordinate frame
\param ang the angle between the world frame and the relative frame
(reasoning from the world frame)
\return the result of converting the current global VecPosition into a
relative VecPosition */
VecPosition VecPosition::GlobalToRelative( VecPosition origin, AngRad ang )
{
	// convert global coordinates into relative coordinates by aligning relative
	// frame and world frame. First perform translation to Make origins of both
	// frames coincide. Then perform rotation to Make axes of both frames coincide
	// (use negative angle since you Rotate relative frame to world frame).
	*this -= origin;
	return ( Rotate( -ang ) );
}

/*! This method converts the coordinates of the current VecPosition (which are
represented in a relative coordinate system) into global coordinates in
the world frame (with origin at (0,0)). The relative coordinate system is
defined by the arguments to the method. The global coordinates are now
obtained by aligning the world frame with the relative frame using a
rotation to align the axes followed by a translation to Make both origins
coincide.
\param origin the origin of the relative coordinate frame
\param ang the angle between the world frame and the relative frame
(reasoning from the world frame)
\return the result of converting the current relative VecPosition into an
global VecPosition */
VecPosition VecPosition::RelativeToGlobal( VecPosition origin, AngRad ang )
{
	// convert relative coordinates into global coordinates by aligning world
	// frame and relative frame. First perform rotation to Make axes of both
	// frames coincide (use positive angle since you Rotate world frame to
	// relative frame). Then perform translation to Make origins of both frames
	// coincide.
	Rotate( ang );
	*this += origin;
	return ( *this );
}

/*! This method returns a VecPosition that lies somewhere on the vector between
the current VecPosition and a given VecPosition. The desired position is
specified by a given fraction of this vector (e.g. 0.5 means exactly in
the middle of the vector). The current VecPosition itself is left unchanged.
\param p a VecPosition which defines the vector to the current VecPosition
\param dFrac double representing the fraction of the connecting vector at
which the desired VecPosition lies.
\return the VecPosition which lies at fraction dFrac on the vector
connecting p and the current VecPosition */
VecPosition VecPosition::GetVecPositionOnLineFraction( VecPosition &p,
double      dFrac )
{
	// determine point on line that lies at fraction dFrac of whole line
	// example: this --- 0.25 ---------  p
	// formula: this + dFrac * ( p - this ) = this - dFrac * this + dFrac * p =