geometry.cpp

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

  return ( VecPosition( m_x / p.m_x, m_y / p.m_y, m_z / p.m_z ) );
}

/*! Overloaded version of the assignment operator for assigning a given double
    value to both the x- and y-coordinates of the current VecPosition. This
    changes the current VecPosition itself.
    \param d a double value which has to be assigned to both the x- and
    y-coordinates of the current VecPosition */
void VecPosition::operator = ( const double &d )
{
  m_x = d;
  m_y = d;
  m_z = d;
}

/*! Overloaded version of the sum-assignment operator for VecPositions. It
    returns the sum of the current VecPosition and the given VecPosition by
    adding their x- and y- and z- coordinates. This changes the current VecPosition
    itself.
    \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;
  m_z += p.m_z;
}

/*! Overloaded version of the sum-assignment 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. This changes the current
    VecPosition itself.
    \param d a double value which has to be added to the x- and
    y- and z-coordinates of the current VecPosition */
void VecPosition::operator += ( const double &d )
{
  m_x += d;
  m_y += d;
  m_z += d;
}

/*! Overloaded version of the difference-assignment operator for
    VecPositions.  It returns the difference between the current
    VecPosition and the given VecPosition by subtracting their x- and
    y-coordinates. This changes the current VecPosition itself.

    \param p a VecPosition which has to be subtracted from the current
    VecPosition */
void VecPosition::operator -=( const VecPosition &p )
{
  m_x -= p.m_x;
  m_y -= p.m_y;
  m_z -= p.m_z;
}

/*! Overloaded version of the difference-assignment 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. This changes the current VecPosition itself.

    \param d a double value which has to be subtracted from the x- and
    y- and z-coordinates of the current VecPosition */
void VecPosition::operator -=( const double &d )
{
  m_x -= d;
  m_y -= d;
  m_z -= d;
}

/*! Overloaded version of the multiplication-assignment operator for
    VecPositions. It returns the product of the current VecPosition
    and the given VecPosition by multiplying their x- and
    y-coordinates. This changes the current VecPosition itself.

    \param p a VecPosition by which the current VecPosition has to be
    multiplied */
void VecPosition::operator *=( const VecPosition &p )
{
  m_x *= p.m_x;
  m_y *= p.m_y;
  m_z *= p.m_z;
}

/*! Overloaded version of the multiplication-assignment 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. This changes the current VecPosition itself.

    \param d a double value by which the x- and y- and z-coordinates of the
    current VecPosition have to be multiplied */
void VecPosition::operator *=( const double &d )
{
  m_x *= d;
  m_y *= d;
  m_z *= d;
}

/*! Overloaded version of the division-assignment operator for
    VecPositions. It returns the quotient of the current VecPosition
    and the given VecPosition by dividing their x- and
    y- and z-coordinates. This changes the current VecPosition itself.

    \param p a VecPosition by which the current VecPosition is divided */
void VecPosition::operator /=( const VecPosition &p )
{
  m_x /= p.m_x;
  m_y /= p.m_y;
  m_z /= p.m_z;
}

/*! Overloaded version of the division-assignment 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. This changes the current VecPosition itself.

    \param d a double value by which both the x- and y- and z-coordinates of the
    current VecPosition have to be divided */
void VecPosition::operator /=( const double &d )
{
  m_x /= d;
  m_y /= d;
  m_z /= d;
}

/*! Overloaded version of the inequality operator for VecPositions. It
    determines whether the current VecPosition is unequal to the given
    VecPosition by comparing their x- and y-coordinates.

    \param p a VecPosition
    \return true when either the x- or y- or z-coordinates of the given VecPosition
    and the current VecPosition are different; false otherwise */
bool VecPosition::operator !=( const VecPosition &p )
{
  return ( ( m_x != p.m_x ) || ( m_y != p.m_y ) || ( m_z != p.m_z ) );
}

/*! Overloaded version of the inequality operator for comparing a
    VecPosition to a double value. It determines whether either the x-
    or y-coordinate of the current VecPosition is unequal to the given
    double value.

    \param d a double value with which both the x- and y-coordinates of the
    current VecPosition have to be compared.
    \return true when either the x- or y- and z-coordinate of the current VecPosition
    is unequal to the given double value; false otherwise */
bool VecPosition::operator !=( const double &d )
{
  return ( ( m_x != d ) || ( m_y != d ) || ( m_z != d ) );
}

/*! Overloaded version of the equality operator for VecPositions. It
    determines whether the current VecPosition is equal to the given
    VecPosition by comparing their x- and y-coordinates.

    \param p a VecPosition
    \return true when the x- and y- and z-coordinates of the given
    VecPosition and the current VecPosition are equal; false
    otherwise */
bool VecPosition::operator ==( const VecPosition &p )
{
  return ( ( m_x == p.m_x ) && ( m_y == p.m_y ) && ( m_z == p.m_z ) );
}

/*! Overloaded version of the equality operator for comparing a
    VecPosition to a double value. It determines whether both the x-
    and y-coordinates of the current VecPosition are equal to the
    given double value.

    \param d a double value with which both the x- and y-coordinates of the
    current VecPosition have to be compared.
    \return true when the x- and y- and z-coordinates of the current VecPosition
    are equal to the given double value; false otherwise */
bool VecPosition::operator ==( const double &d )
{
  return ( ( m_x == d ) && ( m_y == d ) && ( m_z ==d ) );
}

/*! Overloaded version of the C++ output operator for
    VecPositions. This operator makes it possible to use VecPositions
    in output statements (e.g.  cout << v). The x- and y-coordinates
    of the VecPosition are printed in the format (x,y).

    \param os output stream to which information should be written
    \param v a VecPosition which must be printed
    \return output stream containing (x,y,z) */
ostream& operator << ( ostream &os, VecPosition v )
{
  return ( os << "( " << v.m_x << ", " << v.m_y << ", " << v.m_z << " )" );
}

/*! This method writes the current VecPosition to standard output. It
    can also print a polar representation of the current VecPosition.

    \param cs a CoordSystemtT indicating whether a POLAR or CARTESIAN
     representation of the current VecPosition should be printed */
void VecPosition::show( CoordSystemT cs )
{
  if( cs == CARTESIAN )
    cout << *this << endl;
  else
    cout << "( r: " << getMagnitude( ) << ", theta: " << getTheta( ) << ", phi: " << getPhi() << " )";
}

/*! This method writes the current VecPosition to a string. It can
    also write a polar representation of the current VecPosition.

    \param cs a CoordSystemtT indicating whether a POLAR or CARTESIAN
     representation of the current VecPosition should be written
    \return a string containing a polar or Cartesian representation of the
    current VecPosition depending on the value of the boolean argument */
string VecPosition::str( CoordSystemT cs )
{
  char buf[ 1024 ];

  if( cs == CARTESIAN )
    sprintf( buf, "( %f, %f, %f )", getX( ), getY( ), getZ( ) );
  else
    sprintf( buf, "( r: %f, theta: %f, phi: %f )", getMagnitude( ), getTheta( ), getPhi( ) );

  string str( buf );
  return ( str );
}

/*! Set method for the x-coordinate of the current VecPosition.

    \param dX a double value representing a new x-coordinate
    \return a boolean indicating whether the update was successful */
bool VecPosition::setX( double dX )
{
  m_x = dX;
  return ( true );
}

/*! Get method for the x-coordinate of the current VecPosition.

    \return the x-coordinate of the current VecPosition */
double VecPosition::getX( ) const
{
  return ( m_x );
}

/*! Set method for the y-coordinate of the current VecPosition.

    \param dY a double value representing a new y-coordinate
    \return a boolean indicating whether the update was successful */
bool VecPosition::setY( double dY )
{
  m_y = dY;
  return ( true );
}

/*! Get method for the y-coordinate of the current VecPosition.

    \return the y-coordinate of the current VecPosition */
double VecPosition::getY( ) const
{
  return ( m_y );
}

/*! Set method for the z-coordinate of the current VecPosition.

    \param dZ a double value representing a new z-coordinate
    \return a boolean indicating whether the update was successful */
bool VecPosition::setZ( double dZ )
{
  m_z = dZ;
  return ( true );
}

/*! Get method for the z-coordinate of the current VecPosition.

    \return the z-coordinate of the current VecPosition */
double VecPosition::getZ( ) const
{
  return ( m_z );
}

/*! This method (re)sets the coordinates of the current
    VecPosition. The given coordinates can either be polar or
    Cartesian coordinates. This is indicated by the value of the third
    argument.

    \param dX a double value indicating either a new Cartesian
    x-coordinate when cs=CARTESIAN or a new polar r-coordinate
    (distance) when cs=POLAR

    \param dY a double value indicating either a new Cartesian
    y-coordinate when cs=CARTESIAN or a new polar theta-coordinate
    (angle) when cs=POLAR

    \param dZ a double value indicating either a new Cartesian
    z-coordinate when cs=CARTESIAN or a new polar phi-coordinate
    (angle) when cs=POLAR

    \param cs a CoordSystemT indicating whether x and y denote
    cartesian coordinates or polar coordinates */
void VecPosition::setVecPosition( double dX, double dY, double dZ, CoordSystemT cs)
{
  if( cs == CARTESIAN )
  {
    m_x = dX;
    m_y = dY;
    m_z = dZ;
  }
  else
    *this = getVecPositionFromPolar( dX, dY, dZ );
}

/*! This method determines the distance between the current
    VecPosition and a given VecPosition. This is equal to the
    magnitude (length) of the vector connecting the two positions
    which is the difference vector between them.

    \param p a Vecposition
    \return the distance between the current VecPosition and the given
    VecPosition */
double VecPosition::getDistanceTo( const VecPosition p )
{
  return ( ( *this - p ).getMagnitude( ) );
}

/*! This method adjusts the coordinates of the current VecPosition in
    such a way that the magnitude of the corresponding vector equals
    the double value which is supplied as an argument. It thus scales
    the vector to a given length by multiplying both the x- and
    y-coordinates by the quotient of the argument and the current
    magnitude. This changes the VecPosition itself.

    \param d a double value representing a new magnitude

    \return the result of scaling the vector corresponding with the
    current VecPosition to the given magnitude thus yielding a
    different VecPosition */
VecPosition VecPosition::setMagnitude( double d )
{
  if( getMagnitude( ) > EPSILON )
     ( *this ) *= ( d / getMagnitude( ) );

  return ( *this );
}

/*! This method determines the magnitude (length) of the vector
    corresponding with the current VecPosition using the formula of
    Pythagoras.

    \return the length of the vector corresponding with the current
    VecPosition */
double VecPosition::getMagnitude( ) const
{
  return ( sqrt( m_x * m_x + m_y * m_y + m_z * m_z ) );
}

/*! This method determines the direction 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 have to be taken into account in order to