utils.h

浙江大学 RoboCup3D 2006 源代码

	Vector3& operator /=(const double &a) { x /= a;  y /= a; z/=a; return *this;}
	bool operator !=(const Vector3 &a) { return (x != a.x) || (y != a.y) || (z != a.z); }
	bool operator !=(const double &a) { return (x != a) || (y != a) || (z != a); }
	bool operator ==(const Vector3 &a) { return (x == a.x) && (y == a.y) && (z == a.z); }

	// functions
	bool isInOurForbiddenZone();
	bool isInOppForbiddenZone();
	bool isInField(double margine);
	double getDistToOppGoal();
	double getDistToOurGoal();
};

typedef struct tagAgentStatus{
  Vector3 pos;
  Vector3 vel;
}AgentStatus;

typedef struct tagBallStatus{
  Vector3 pos;
  Vector3 vel;
}BallStatus;

int isInSector(Vector3 vecA, Vector3 vecB, double ang, double mod); //add by zy



/******************************************************************************/
/*********************   CLASS GEOMETRY   *************************************/
/******************************************************************************/

/*! This class contains several static methods dealing with geometry.*/
class Geometry
{

public:

  // geometric series
  static double getLengthGeomSeries(double dFirst,double dRatio,double dSum   );
  static double getSumGeomSeries   (double dFirst,double dRatio,double dLength);
  static double getSumInfGeomSeries(double dFirst,double dRatio               );
  static double getFirstGeomSeries (double dSum,  double dRatio,double dLength);
  static double getFirstInfGeomSeries(double dSum,double dRatio               );

  // abc formula
  static int    abcFormula(double a,double b, double c, double *s1, double *s2);
};

/******************************************************************************/
/********************** CLASS CIRCLE ******************************************/
/******************************************************************************/

/*!This class represents a circle. A circle is defined by one VecPosition
   (which denotes the center) and its radius. */
class Circle
{
    Vector3		m_posCenter;            /*!< Center of the circle  */
    double      m_dRadius;              /*!< Radius of the circle  */

public:
    Circle( );
    Circle( Vector3 pos, double dR );

    void        show                  ( ostream& os = cout );

    // get and set methods
    bool        setCircle             ( Vector3 pos, 
                                        double      dR  );
    bool        setRadius             ( double dR       );
    double      getRadius             (                 );
    bool        setCenter             ( Vector3 pos     );
    Vector3		getCenter             (                 );
    double      getCircumference      (                 );
    double      getArea               (                 );

    // calculate intersection points and area with other circle
    bool        isInside              ( Vector3 pos     );
    int         getIntersectionPoints ( Circle      c, 
                                        Vector3 *p1, 
                                        Vector3 *p2     );
    double      getIntersectionArea   ( Circle c        );


}  ;


/*****************************************************************************/
/*********************** CLASS LINE ******************************************/
/*****************************************************************************/

/*!This class contains the representation of a line. A line is defined
   by the formula ay + bx + c = 0. The coefficients a, b and c are stored
   and used in the calculations. */
class Line
{
protected:
  // a line is defined by the formula: ay + bx + c = 0
  double m_a; /*!< This is the a coefficient in the line ay + bx + c = 0 */
  double m_b; /*!< This is the b coefficient in the line ay + bx + c = 0 */
  double m_c; /*!< This is the c coefficient in the line ay + bx + c = 0 */

public:
  Line(){};
  Line( double a, double b, double c );

  // print methods
  void        show( ostream& os = cout );
  friend      ostream& operator << (ostream & os, Line l);

  // get intersection points with this line
  Vector3     getIntersection            ( Line        line                  );
  int         getCircleIntersectionPoints( Circle      circle,
                                           Vector3     *posSolution1,
                                           Vector3 *posSolution2         );
  Line        getTangentLine             ( Vector3 pos                   );
  Vector3     getPointOnLineClosestTo    ( Vector3 pos                   );
  double      getDistanceWithPoint       ( Vector3 pos                   );
  bool        isInBetween                ( Vector3 pos,
                                           Vector3 point1,
                                           Vector3 point2                );

  // calculate associated variables in the line
  double      getYGivenX                 ( double      x );
  double      getXGivenY                 ( double      y );
  double      getACoefficient            (               ) const;
  double      getBCoefficient            (               ) const;
  double      getCCoefficient            (               ) const;

  // static methods to make a line using an easier representation.
  static Line makeLineFromTwoPoints      ( Vector3 pos1,
                                           Vector3 pos2                  );
  static Line makeLineFromPositionAndAngle( Vector3 vec,
                                           Angle angle                      );
};

/******************************************************************************/
/********************** CLASS RECTANGLE ***************************************/
/******************************************************************************/

/*!This class represents a rectangle. A rectangle is defined by two VecPositions
   the one at the upper left corner and the one at the right bottom. */
class Rectangle_
{
  Vector3 m_posLeftTop;     /*!< top left position of the rectangle       */
  Vector3 m_posRightBottom; /*!< bottom right position of the rectangle   */

public:
  Rectangle_                     ( Vector3 pos, Vector3 pos2 );

  void        show              ( ostream& os = cout                );

  // checks whether point lies inside the rectangle
  bool        isInside          ( Vector3 pos                   );

  // standard get and set methosd
  void        setRectanglePoints( Vector3 pos1,
                                  Vector3 pos2                  );
  bool        setPosLeftTop     ( Vector3 pos                   );
  Vector3 getPosLeftTop     ( Vector3 pos                   );
  bool        setPosRightBottom ( Vector3 pos                   );
  Vector3 getPosRightBottom ( Vector3 pos                   );
};

class Ray:public Line
{
  Vector3 m_origin;/*射线的初始构造点*/
  Angle m_angle;/*射线的角度*/
  Vector3 m_direction;/*射线的单位方向*/

public:
	Ray(){}
  	Ray(const Vector3& orig, double ang);

  Ray(double a,double b,double c,Vector3 pos,Angle angle);

  Angle Allowed_AngleDiff(double distance=5.0);/*允许的角度差*/
  Angle Allowed_AngleDiff(Vector3 point);/*允许的角度差*/
  bool	 InRightDir(Vector3 point);/*判断点是否在射线的方向上*/
  bool	 InRightDir(Angle ang);/*判断角度是否在射线的方向上*/
  bool	 InOppositeDir(Vector3 point);/*判断点是否在射线的反方向上*/
  bool   InOppositeDir(Angle ang);/*判断角度是否在射线的反方向上*/
  Vector3 GetPoint(double dist){ return m_origin + m_direction * dist;}
  double  getDistanceFromOrigin(Vector3 pos);
  inline Angle GetRayAngle(){return m_angle;}
  inline Vector3 GetRayDirection(){return m_direction;}
  static Ray	 MakeRayFromPositionAndAngle(Vector3 pos,Angle angle);/*产生射线*/
  int CircleIntersect(double radius, const Vector3& center, Vector3& psol1, Vector3& psol2) const;
};
/***************************************************************************/
class CAngle
{
public:
	CAngle(){};
	~CAngle(){};
	CAngle(Vector3 p,Vector3 begin,Vector3 end,Vector3 bisector);
	CAngle(Vector3 p,double begin,double end,double bisector);
	//======
	Vector3 p;
	Angle ang1,ang2;
	Angle bisector;
	//======
	int getIntersection(CAngle angle,CAngle& angout1,CAngle &angout2);
	bool isNormal();
	
};

Angle Normalize(Angle ang);
double getSumGeomSeries( double dFirst, double dRatio, double dLength);
bool isContain(const char* whole,const char* sub);

#endif