vec.cpp

浙江大学 RoboCup3D 2006 源代码

}

//-------------------------------------------------------------

//--------------------------------------------------------------
/*! 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 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( Angle 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, Angle 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, Angle 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, Angle 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 */
Angle VecPosition::normalizeAngle( Angle angle )
{
  while( angle > 180.0  ) angle -= 360.0;
  while( angle < -180.0 ) angle += 360.0;

  return ( angle );
}
bool VecPosition::isInField(double margine)
{
	return (bool)(fabs(m_x)<52.5 - margine && fabs(m_y) < 34 - margine);
}