geometry.cpp

2003年RoboCup仿真组世界冠军源代码 足球机器人 仿真组 的源代码

    VecPosition; false otherwise */
bool VecPosition::isBehindOf( 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::isBehindOf( 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::isLeftOf( 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::isLeftOf( 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::isRightOf( 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::isRightOf( 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 ( ( isInFrontOf( p1 ) && isBehindOf( 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 ( ( isInFrontOf( d1 ) && isBehindOf( 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 ( ( isRightOf( p1 ) && isLeftOf( 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 ( ( isRightOf( d1 ) && isLeftOf( 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( AngDeg 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, AngDeg 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, AngDeg 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 =
  //          ( 1 - dFrac ) * this + dFrac * p
  return ( ( *this ) * ( 1.0 - dFrac ) + ( p * dFrac ) );
}

/*! This method converts a polar representation of a VecPosition into
    a Cartesian representation.

    \param dMag a double representing the polar r-coordinate, i.e. the
    distance from the point to the origin

    \param ang the angle that the polar vector makes with the x-axis,
    i.e. the polar phi-coordinate

    \return the result of converting the given polar representation
    into a Cartesian representation thus yielding a Cartesian
    VecPosition */
VecPosition VecPosition::getVecPositionFromPolar( double dMag, AngDeg ang )
{
  // cos(phi) = x/r <=> x = r*cos(phi); sin(phi) = y/r <=> y = r*sin(phi)
  return ( VecPosition( dMag * cosDeg( ang ), dMag * sinDeg( ang ) ) );
}

/*! This method normalizes an angle. This means that the resulting
    angle lies between -180 and 180 degrees.

    \param angle the angle which must be normalized

    \return the result of normalizing the given angle */
AngDeg VecPosition::normalizeAngle( AngDeg angle )
{
  while( angle > 180.0  ) angle -= 360.0;
  while( angle < -180.0 ) angle += 360.0;

  return ( angle );
}


/*****************************************************************************/
/********************** CLASS GEOMETRY ***************************************/
/*****************************************************************************/

/*! A geometric series is one in which there is a constant ratio between each
    element and the one preceding it. This method determines the
    length of a geometric series given its first element, the sum of the
    elements in the series and the constant ratio between the elements.
    Normally: s = a + ar + ar^2 + ...  + ar^n
    Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2 + .. + dFist*dRatio^n
    \param dFirst first term of the series
    \param dRatio ratio with which the the first term is multiplied
    \param dSum the total sum of all the serie
    \return the length(n in above example) of the series */
double Geometry::getLengthGeomSeries( double dFirst, double dRatio, double dSum )
{
  if( dRatio < 0 )
    cerr << "(Geometry:getLengthGeomSeries): negative ratio" << endl;

  // s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
  // subtract: sr - s = - a + ar^n) =>  s(1-r)/a + 1 = r^n = temp
  // log r^n / n = n log r / log r = n = length
  double temp = (dSum * ( dRatio - 1 ) / dFirst) + 1;
  if( temp <= 0 )
    return -1.0;
  return log( temp ) / log( dRatio ) ;
}

/*! A geometric series is one in which there is a constant ratio between each
    element and the one preceding it. This method determines the sum of a
    geometric series given its first element, the ratio and the number of steps
    in the series
    Normally: s = a + ar + ar^2 + ...  + ar^n
    Now: dSum = dFirst + dFirst*dRatio + ... + dFirst*dRatio^dSteps
    \param dFirst first term of the series
    \param dRatio ratio with which the the first term is multiplied
    \param dSum the number of steps to be taken into account
    \return the sum of the series */
double Geometry::getSumGeomSeries( double dFirst, double dRatio, double dLength)
{
  // s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
  // subtract: s - sr = a - ar^n) =>  s = a(1-r^n)/(1-r)
  return dFirst * ( 1 - pow( dRatio, dLength ) ) / ( 1 - dRatio ) ;
}

/*! A geometric series is one in which there is a constant ratio between each
    element and the one preceding it. This method determines the sum of an
    infinite geometric series given its first element and the constant ratio
    between the elements. Note that such an infinite series will only converge
    when 0<r<1.
    Normally: s = a + ar + ar^2 + ar^3 + ....
    Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2...
    \param dFirst first term of the series
    \param dRatio ratio with which the the first term is multiplied
    \return the sum of the series */
double Geometry::getSumInfGeomSeries( double dFirst, double dRatio )
{
  if( dRatio > 1 )
    cerr << "(Geometry:CalcLengthGeomSeries): series does not converge" <<endl;