vec.cpp

浙江大学 RoboCup3D 2006 源代码

/***************************************************************************
 *   Copyright (C) 2004 - 2006 by ZJUBase                                  *
 *                                National Lab of Industrial Control Tech. *
 *                                Zhejiang University, China               *

*                                                                         *
 *   Team members:                                                         *
 *    Currently the team leader is,                                        *
 *           Hao JIANG ([e-mail]; [e-mail])       *
 *    In the next season, the leader will be                               *
 *           Yifeng ZHANG ([e-mail])                        *
 *    ZJUBase 3D agent is created by                                       *
 *           Dijun LUO ([e-mail])                             *
 *    All the members who has ever contributed:                            *
 *           Jun JIANG                                                     *
 *           Xinfeng DU ([e-mail])                             *
 *           Yang ZHOU ([e-mail])                             *
 *           Zhipeng YANG                                                  *
 *           Xiang FAN                                                     *
 *                                                                         *
 *   Team Manager:                                                          *
 *      Ms. Rong XIONG ([e-mail])                            *
 *                                                                         *
 *   If you met any problems or you have something to discuss about        *
 *   ZJUBase. Please feel free to contact us through EMails given below.   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "main.h"
#define EPSILON 0.00001
Angle Rad2Deg( double x )
{
  return ( x * 180 / 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 */
double Deg2Rad( Angle x )
{
  return ( x * 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( Angle 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( Angle 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( Angle 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 */
Angle 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 */
Angle 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 */
Angle asinDeg( double x )
{
  if( x >= 1 )
    return ( 90.0 );
  else if ( x <= -1 )
    return ( -90.0 );

  return ( Rad2Deg( asin( x ) ) );
}

void VecPosition::show( CoordSystemT cs )
{
}

/*! 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 )", getX( ), getY( ) );
  else
    sprintf( buf, "( r: %f, phi: %f )", getMagnitude( ), getDirection( ) );

  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 );
}

/*! 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 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 , CoordSystemT cs)
{
  if( cs == CARTESIAN )
  {
    m_x = dX;
    m_y = dY;
  }
  else
    *this = getVecPositionFromPolar( dX, dY );
}

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

/*! 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 determine the correct quadrant.
    \return the direction in degrees of the vector corresponding with the
    current VecPosition */
Angle VecPosition::getDirection( ) const
{
  return ( atan2Deg( m_y, m_x ) );
}

//-------------------------------------------------------------
Angle VecPosition::getDirectionTo(const VecPosition &p)
{
	return (*this - p).getDirection();