/*Copyright (c) 2000-2002, Jelle Kok, University of AmsterdamAll rights reserved.Redistribution and use in source and binary forms, with or withoutmodification, are permitted provided that the following conditions are met:1. Redistributions of source code must retain the above copyright notice, thislist of conditions and the following disclaimer.2. Redistributions in binary form must reproduce the above copyright notice,this list of conditions and the following disclaimer in the documentationand/or other materials provided with the distribution.3. Neither the name of the University of Amsterdam nor the names of itscontributors may be used to endorse or promote products derived from thissoftware without specific prior written permission.THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THEIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AREDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNE
R OR CONTRIBUTORS BE LIABLEFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIALDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS ORSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVERCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USEOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*//*! \file Geometry.C<pre><b>File:</b>          Geometry.C<b>Project:</b>       Robocup Soccer Simulation Team: UvA Trilearn<b>Authors:</b>       Jelle Kok<b>Created:</b>       13/02/2001<b>Last Revision:</b> $ID$<b>Contents:</b>      class declarations of different geometry classes:<BR>- VecPosition: representation of a point- Line:        representation of a line- Rectangle:   representation of a rectangle- Circle:      representation of a circle- Geometry:    different geometry methodsFurthermore it contains some goniometric functions to work with sine, cosineand tangent functions using degrees and some utility functions to returnthe maximum and the minimum of two values.<hr size=2><h2><b>Changes</b></h2><b>Date</b>             <b>Author</b>          <b>Comment</b>12/02/2001       Jelle Kok       Initial version created</pre>*/#include "stdafx.h"#include "GeometryR.h"#include <stdio.h>    // needed for sprintf#include <limits.h>/*******************************************************************************************//** Following is Math functions ************************************************************//*******************************************************************************************//*! This function returns the Maths::Sign of a give double.1 is positive, -1 is negative\param d1 first parameter\return the Maths::Sign of this double */int Maths::Sign( double d1 ){	return (d1>0)?1:-1;}
int Maths::Round(double d)
{
	if (d<0)
		return Round(d+INT_MAX)-INT_MAX;
	else
		return static_cast<int>(d+0.5);
}

double Maths::Limit( double d, double dMin, double dMax)
{
	if (dMin > dMax)
	{
		double dTemp = dMin;
		dMin = dMax;
		dMax = dTemp;
	}

	if (d < dMin) d = dMin;
	if (d > dMax) d = dMax;
	return d;
}
/*! 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 Maths::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 Maths::Min( double d1, double d2 ){	return (d1<d2)?d1:d2;}/*! This function converts an angle in radians to the corresponding angle indegrees.\param x an angle in radians\return the corresponding angle in degrees */AngDeg Maths::Rad2Deg( AngRad x ){	return ( x * 180 / M_PI );}/*! This function converts an angle in degrees to the corresponding angle inradians.\param x an angle in degrees\return the corresponding angle in radians */AngRad Maths::Deg2Rad( AngDeg x ){	return ( x * M_PI / 180 );}/*! This function returns the cosine of a given angle in degrees using thebuilt-in cosine function that works with angles in radians.\param x an angle in degrees\return the cosine of the given angle */double Maths::cosDeg( AngDeg x ){	return ( cos( Deg2Rad( x ) ) );}/*! This function returns the sine of a given angle in degrees using thebuilt-in sine function that works with angles in radians.\param x an angle in degrees\return the sine of the given angle */double Maths::sinDeg( AngDeg x ){	return ( sin( Deg2Rad( x ) ) );}/*! This function returns the tangent of a given angle in degrees using thebuilt-in tangent function that works with angles in radians.\param x an angle in degrees\return the tangent of the given angle */double Maths::tanDeg( AngDeg x ){	return ( tan( Deg2Rad( x ) ) );}/*! This function returns the principal value of the arc tangent of x in degreesusing 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 Maths::atanDeg( double x ){	return ( Rad2Deg( atan( x ) ) );}/*! This function returns the principal value of the arc tangent of y/x indegrees using the signs of both arguments to determine the quadrant of thereturn value. For this the built-in 'atan2' function is used which returnsthis 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 intoaccount */double Maths::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 degreesusing 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 Maths::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 degreesusing 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 Maths::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 falseIsAngInInterval(   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( AngRad ang, AngRad angMin, AngRad angMax ){	// convert all angles to interval 0..360	if( ( ang    + 2*M_PI ) < 2*M_PI ) ang    += 2*M_PI;	if( ( angMin + 2*M_PI ) < 2*M_PI ) angMin += 2*M_PI;	if( ( angMax + 2*M_PI ) < 2*M_PI ) angMax += 2*M_PI;	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 dealswith the boundary problem, thus when 'angMin' equals 170 and 'angMax'equals -100, -145 is returned.\param angMin minimum angle [-PI,PI]\param angMax maximum angle [-PI,PI]\return average of angMin and angMax. */AngRad GetBisectorTwoAngles( AngRad angMin, AngRad angMax ){	// separate sine and cosine part to circumvent boundary problem	return VecPosition::NormalizeAngle(	atan2( (sin( angMin) + sin( angMax ) )/2.0,	(cos( angMin) + cos( angMax ) )/2.0 ) );}/******************************************************************************//********************   CLASS VECPOSITION   ***********************************//******************************************************************************//*! Constructor for the VecPosition claSS-> When the supplied Coordinate Systemtype equals CARTESIAN, the arguments x and y denote the x- and y-coordinatesof the new position. When it equals POLAR however, the arguments x and ydenote the polar coordinates of the new position; in this case x is thusequal to the distance r from the origin and y is equal to the angle phi thatthe polar vector Makes with the x-axis.\param x the x-coordinate of the new position when cs == CARTESIAN; thedistance of the new position from the origin when cs = POLAR\param y the y-coordinate of the new position when cs = CARTESIAN; theangle that the polar vector Makes with the x-axis when cs = POLAR\param cs a CoordSystemT indicating whether x and y denote cartesiancoordinates 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 thenegative 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 doublevalue to a VecPosition. The double value is added to both the x- andy-coordinates of the current VecPosition. The current VecPosition itself isleft unchanged.\param d a double value which has to be added to both the x- andy-coordinates of the current VecPosition\return the result of adding the given double value to the currentVecPosition */VecPosition VecPosition::operator + ( const double &d ){	return ( VecPosition( m_x + d, m_y + d ) );}/*! Overloaded version of the binary plus operator for VecPositions. It returnsthe sum of the current VecPosition and the given VecPosition by adding theirx- 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 givendouble value from a VecPosition. The double value is subtracted from boththe x- and y-coordinates of the current VecPosition. The current VecPositionitself is left unchanged.\param d a double value which has to be subtracted from both the x- andy-coordinates of the current VecPosition\return the result of subtracting the given double value from the currentVecPosition */VecPosition VecPosition::operator - ( const double &d ){	return ( VecPosition( m_x - d, m_y - d ) );}/*! Overloaded version of the binary minus operator for VecPositions. It returnsthe difference between the current VecPosition and the given VecPosition bysubtracting their x- and y-coordinates. The VecPositions themselves are leftunchanged.\param p a VecPosition\return the difference between the current VecPosition and the givenVecPosition */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 aVecPosition by a given double value. Both the x- and y-coordinates of thecurrent VecPosition are multiplied by this value. The current VecPositionitself is left unchanged.\param d the multiplication factor\return the result of multiplying the current VecPosition by the givendouble value */VecPosition VecPosition::operator * ( const double &d  ){	return ( VecPosition( m_x * d, m_y * d  ) );}/*! Overloaded version of the multiplication operator for VecPositions. Itreturns the product of the current VecPosition and the given VecPosition bymultiplying their x- and y-coordinates. The VecPositions themselves are leftunchanged.\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 agiven double value. Both the x- and y-coordinates of the current VecPositionare 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 doublevalue */VecPosition VecPosition::operator / ( const double &d ){	return ( VecPosition( m_x / d, m_y / d  ) );}/*! Overloaded version of the division operator for VecPositions. It returns thequotient of the current VecPosition and the given VecPosition by dividingtheir x- and y-coordinates. The VecPositions themselves are left unchanged.\param p a VecPosition\return the quotient 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 assignment operator for assigning a given doublevalue to both the x- and y-coordinates of the current VecPosition. Thischanges the current VecPosition itself.\param d a double value which has to be assigned to both the x- andy-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. Itreturns the sum of the current VecPosition and the given VecPosition byadding their x- and y-coordinates. This changes the current VecPositionitself.\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 doublevalue to a VecPosition. The double value is added to both the x- andy-coordinates of the current VecPosition. This changes the currentVecPosition itself.\param d a double value which has to be added to both the x- andy-coordinates of the current VecPosition */void VecPosition::operator += ( const double &d )