geometry.cpp

robocup 3d, a 3d base team similar to UvA 2d

    determine the correct quadrant.

    \return the direction in degrees of the vector corresponding with
    the current VecPosition */
AngDeg VecPosition::getDirection( ) const
{
  return ( atan2Deg( m_y, m_x ) );
}

/*! This method determines the theta of the vector corresponding
    with the current VecPosition (the theta-coordinate in polar
    representation) using the arc tangent function. Note that the
    signs of x and y and z have to be taken into account in order to
    determine the correct quadrant.

    \return the theta in degrees of the vector corresponding with
    the current VecPosition */
AngDeg VecPosition::getTheta( ) const
{
  return ( atan2Deg( m_y, m_x ) );
}

/*! This method determines the phi 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 and z have to be taken into account in order to
    determine the correct quadrant.

    \return the phi in degrees of the vector corresponding with
    the current VecPosition */
AngDeg VecPosition::getPhi( ) const
{
  double dXYPict = sqrt( m_x * m_x + m_y * m_y );
  return ( atan2Deg( m_z, dXYPict ) );
}

/*! 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::isInFrontOf( 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::isInFrontOf( 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::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 to the
    up of a given VecPosition, i.e. whether the z-coordinate of the
    current VecPosition is larger than the z-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 up of the
    given VecPosition; false otherwise */
bool VecPosition::isUpOf( const VecPosition &p )
{
  return ( ( m_z > p.getZ( ) ) ? true : false );
}

/*! This method determines whether the z-coordinate of the current
    VecPosition is to the up of (i.e. larger than) a given double
    value.

    \param d a double value to which the current z-coordinate must be
    compared

    \return true when the current z-coordinate is to the right of the
    given value; false otherwise */
bool VecPosition::isUpOf( const double &d )
{
  return ( ( m_z > d ) ? true : false );
}

/*! This method determines whether the current VecPosition is to the
    down of a given VecPosition, i.e. whether the z-coordinate of the
    current VecPosition is less than the z-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 down of the
    given VecPosition; false otherwise */
bool VecPosition::isDownOf( const VecPosition &p )
{
  return ( ( m_z < p.getZ( ) ) ? true : false );
}

/*! This method determines whether the z-coordinate of the current
    VecPosition is to the down of (i.e. less than) a given double
    value.

    \param d a double value to which the current z-coordinate must be
    compared

    \return true when the current z-coordinate is to the right of the
    given value; false otherwise */
bool VecPosition::isDownOf( const double &d )
{
  return ( ( m_z < 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 determines whether the current VecPosition is in
    between two given VecPositions when looking in the z-direction,
    i.e. whether the current VecPosition is in up of the first
    argument and down 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 z-direction; false
    otherwise */
bool VecPosition::isBetweenZ( const VecPosition &p1, const VecPosition &p2 )
{
  return ( ( isUpOf( p1 ) && isDownOf( p2 ) ) ? true : false );
}

/*! This method determines whether the z-coordinate of the current
    VecPosition is in between two given double values, i.e. whether
    the z-coordinate of the current VecPosition is upper than the
    first argument and down the second.

    \param d1 a double value to which the current z-coordinate must be
    compared

    \param d2 a double value to which the current z-coordinate must be
    compared

    \return true when the current z-coordinate is in between the two
    given values; false otherwise */
bool VecPosition::isBetweenZ( const double &d1, const double &d2 )
{
  return ( ( isUpOf( d1 ) && isDownOf( 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 theta
  setVecPosition( dMag, dNewDir, this->getPhi( ), 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,0)) into relative coordinates in a different