geometry.cpp

自己写的robocup-2d程序

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All 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.cpp
<pre>
<b>File:</b>          Geometry.cpp
<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 methods

Furthermore it contains some goniometric functions to work with sine, cosine
and tangent functions using degrees and some utility functions to return
the 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

#ifndef M_PI
#define M_PI 3.141593
#endif

/*! 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;
}

#ifndef WIN32

/*! 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;
}

#endif

/*! 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 */