/*Copyright (c) 2000-2002, Jelle Kok, University of AmsterdamAll rights reserved.Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:1. Redistributions of source code must retain the above copyright notice, this list 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 documentation and/or other materials provided with the distribution. 3. Neither the name of the University of Amsterdam nor the names of its contributors may be used to endorse or promote products derived from this software 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, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 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 "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  ) );}