geometry.h

3D仿真组实物机器人环境下的机器人模型的设计工具。可以查看和修改现有模型的详细参数

/*
*********************************************************************************
*                            In the name of Almighty                            *
*                                                                               *
*            Geometry.h : Robocup 3D Soccer Simulation Team Zigorat             *
*                     (This team was previously named Gcyrus)                   *
*                                                                               *
*  Date: 03/20/2007                                                             *
*  Author: Mahdi Hamdarsi                                                       *
*  Comments: class declarations of different geometry classes                   *
*            Some materials of this files are owned & copyrighted by Jelle Kok  *
*                                                                               *
*********************************************************************************
*/

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the University of Amsterdam nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*! \file Geometry.h
<pre>
<b>File:</b>          Geomtery.h
<b>Project:</b>       Robocup Soccer Simulation Team: Zigorat
<b>Authors:</b>       Mahdi Hamdarsi
<b>Created:</b>       03/20/2007
<b>Last Revision:</b> $ID$
<b>Contents:</b>      class declarations of different geometry classes
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
03/20/2007       Mahdi           Initial version created
</pre>
*/


#ifndef _GEOMETRY_
#define _GEOMETRY_

#include <math.h>       // needed for M_PI constant
#include <string>       // needed for string
#include <iostream>

using namespace std;

typedef double AngRad;  /*!< Type definition for angles in degrees. */
typedef double AngDeg;  /*!< Type definition for angles in radians. */

#define EPSILON 0.0001  /*!< Value used for floating point equality tests. */

// auxiliary numeric functions for determining the
// maximum and minimum of two given double values and the sign of a value
double max     ( double d1, double d2 );
double min     ( double d1, double d2 );
int    sign    ( double d1            );

// auxiliary goniometric functions which enable you to
// specify angles in degrees rather than in radians
AngDeg Rad2Deg ( AngRad x             );
AngRad Deg2Rad ( AngDeg x             );
double cosDeg  ( AngDeg x             );
double sinDeg  ( AngDeg x             );
double tanDeg  ( AngDeg x             );
AngDeg atanDeg ( double x             );
double atan2Deg( double x,  double y  );
AngDeg acosDeg ( double x             );
AngDeg asinDeg ( double x             );

// various goniometric functions
bool   isAngInInterval     ( AngDeg ang,    AngDeg angMin,    AngDeg angMax );
AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax );

const double UnknownDoubleValue = NAN;

/*! CoordSystem is an enumeration of the different specified
    coordinate systems.  The two possibilities are CARTESIAN or
    POLAR. These values are for instance used in the initializing a
    VecPosition. The CoordSystem indicates whether the supplied
    arguments represent the position in cartesian or in polar
    coordinates. */
enum CoordSystemT {
  CARTESIAN,
  POLAR
};

/*****************************************************************************/
/********************   CLASS VECPOSITION   **********************************/
/*****************************************************************************/

/*! This class contains an x- and y-coordinate of a position (x,y) as
    member data and methods which operate on this position. The
    standard arithmetic operators are overloaded and can thus be
    applied to positions (x,y). It is also possible to represent a
    position in polar coordinates (r,phi), since the class contains a
    method to convert these into cartesian coordinates (x,y). */
class VecPosition
{
  // private member data
private:

  double m_x;   /*!< x-coordinate of this position */
  double m_y;   /*!< y-coordinate of this position */
  double m_z;   /*!< z-coordinate of this position */

  // public methods
public:
  // constructor for VecPosition class
  VecPosition                               ( double            vx = 0,
                                              double            vy = 0,
                                              double            vz = 0,
                                              CoordSystemT      cs =CARTESIAN);

  // overloaded arithmetic operators
  VecPosition        operator -             (                                );
  VecPosition        operator +             ( const double      &d           );
  VecPosition        operator +             ( const VecPosition &p           );
  VecPosition        operator -             ( const double      &d           );
  VecPosition        operator -             ( const VecPosition &p           );
  VecPosition        operator *             ( const double      &d           );
  VecPosition        operator *             ( const VecPosition &p           );
  VecPosition        operator /             ( const double      &d           );
  VecPosition        operator /             ( const VecPosition &p           );
  void               operator =             ( const double      &d           );
  void               operator +=            ( const VecPosition &p           );
  void               operator +=            ( const double      &d           );
  void               operator -=            ( const VecPosition &p           );
  void               operator -=            ( const double      &d           );
  void               operator *=            ( const VecPosition &p           );
  void               operator *=            ( const double      &d           );
  void               operator /=            ( const VecPosition &p           );
  void               operator /=            ( const double      &d           );
  bool               operator !=            ( const VecPosition &p           ) const;
  bool               operator !=            ( const double      &d           ) const;
  bool               operator ==            ( const VecPosition &p           ) const;
  bool               operator ==            ( const double      &d           ) const;

  // methods for producing output
  friend ostream&    operator <<            ( ostream           &os,
                                              VecPosition       p            );
  void               show                   ( CoordSystemT      cs =CARTESIAN);
  string             str                    ( CoordSystemT      cs =CARTESIAN);

  // set- and get methods for private member variables
  bool               setX                   ( double            dX           );
  double             getX                   (                          ) const;
  bool               setY                   ( double            dY           );
  double             getY                   (                          ) const;
  bool               setZ                   ( double            dZ           );
  double             getZ                   (                          ) const;

  // set- and get methods for derived position information
  void               setVecPosition         ( double            dX = 0,
                                              double            dY = 0,
                                              double            dZ = 0,
                                              CoordSystemT      cs =CARTESIAN);
  double             getDistanceTo          ( const VecPosition p            );
  VecPosition        setMagnitude           ( double            d            );
  double             getMagnitude           (                                ) const;
  AngDeg             getDirection           (                                ) const;
  AngDeg             getTheta               (                                ) const;
  AngDeg             getPhi                 (                                ) const;

  // comparison methods for positions
  bool               isInFrontOf            ( const VecPosition &p           );
  bool               isInFrontOf            ( const double      &d           );
  bool               isBehindOf             ( const VecPosition &p           );
  bool               isBehindOf             ( const double      &d           );
  bool               isLeftOf               ( const VecPosition &p           );
  bool               isLeftOf               ( const double      &d           );
  bool               isRightOf              ( const VecPosition &p           );
  bool               isRightOf              ( const double      &d           );
  bool               isUpOf                 ( const VecPosition &p           );
  bool               isUpOf                 ( const double      &d           );
  bool               isDownOf               ( const VecPosition &p           );
  bool               isDownOf               ( const double      &d           );
  bool               isBetweenX             ( const VecPosition &p1,
                                              const VecPosition &p2          );
  bool               isBetweenX             ( const double      &d1,
                                              const double      &d2          );
  bool               isBetweenY             ( const VecPosition &p1,
                                              const VecPosition &p2          );
  bool               isBetweenY             ( const double      &d1,
                                              const double      &d2          );
  bool               isBetweenZ             ( const VecPosition &p1,
                                              const VecPosition &p2          );