/***************************************************************************************** *                                      SEU-3D *                     ------------------------------------------------- * Copyright (c) 2005, Yuan XU<xychn15@yahoo.com.cn>,Chang'e SHI<evelinesce@yahoo.com.cn> * Copyright (c) 2006, Yuan XU<xuyuan.cn@gmail.com>,Chunlu JIANG<JamAceWatermelon@gmail.com> * Southeast University ,China * All rights reserved. * * Additionally,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 Library 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. ****************************************************************************************/ #ifndef _GEOMETRY_H#define _GEOMETRY_H#ifdef __cplusplusextern "C"{#endif#ifdef __cplusplus}#endif#include <algorithm>#include "math.h"       // needed for M_PI constant#include <string>#include <map>#include <soccer/soccertypes.h>#include "Settings.h"#include <deque>using namespace std;using namespace salt;#define EPSILON 0.0001  // Value used for floating point equality tests. // auxiliary numeric functions for determining the// the sign of a value/*! This function returns the sign of a give value.    1 is positive, -1 is negative    \param d1 first parameter    \return the sign of this value */template < class Type >int sign( Type d1 ){  return (d1>0)?1:-1;}//-* random value in (min,max)template < class Type >Type random( Type min, Type max ){	//if ( min > max ) swap(min,max);//no need :)	srand(time(0));	return (Type(rand())/RAND_MAX)*(max-min)+min;}// auxiliary goniometric functions which enable you to// specify angles in degrees rather than in radiansAngDeg Rad2Deg ( AngRad x );AngRad Deg2Rad ( AngDeg x );float cosDeg  ( AngDeg x );float sinDeg  ( AngDeg x );float tanDeg  ( AngDeg x );AngDeg atanDeg ( float x );AngDeg atan2Deg ( float y, float x );AngDeg acosDeg ( float x );AngDeg asinDeg ( float x );//-* various goniometric functionsbool   isAngInInterval     ( AngDeg ang, AngDeg angMin, AngDeg angMax );AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax );/////////////////// normalize ///////////////////AngDeg normalizeAngle( AngDeg angle );Vector3f normalizeVector3f( const Vector3f &v, const float maxLength );Vector3f normalizeDriveForce( Vector3f driveForce );float normalizeKickForce( float kickForce );AngDeg normalizeKickAngle( AngDeg kickAngle );struct VisionSense{    /** distance perceptor to object */    float distance;	/** theta is the angle in the X-Y (horizontal) plane */	AngDeg theta;	/** phi is the latitude angle */	AngDeg phi;		//VisionSense() : distance(0), theta(0), phi(0) {};	VisionSense() : distance(1000.0), theta(0), phi(0) {};	VisionSense( const float &d, const AngDeg &t, const AngDeg &p): distance(d), theta(t), phi(p) {};};//the vision is relative pos in formate POLAR//the posRelative is relative pos in formate CARTESIAN(X-Y-Z)Vector3f getPosRelativeFromVision( VisionSense vision );VisionSense getPosVisionFromRelative( const Vector3f &posRelative );typedef VisionSense Polar;#define pol2xyz getPosRelativeFromVision#define xyz2pol getPosVisionFromRelative//-* some function about Vector3fVector3f setVector3fLength(Vector3f v,float length);AngDeg	 getClipAng(const Vector3f &v1, const Vector3f &v2);AngDeg	 getClipAng(const Vector3f &p0, const Vector3f &p1, const Vector3f &p2);bool	 isThreePointOneLine( Vector3f p1, Vector3f p2, Vector3f p3 , AngDeg angThr = 1.8 );AngDeg	 getVector3fHorizontalAng( Vector3f v );float	 getDistToLine( Vector3f p, Vector3f l1, Vector3f l2 );//////////////////// template ///////////////////////////////template < class Type >	Type setMaxNMin( const Type x, const Type max, const Type min ){	if ( x > max ) return max;	if ( x < min ) return min;	return x;}/** set the value to range [min,max] *  @param[in] x the orginal value *  @param[in] min the range left *  @param[in] max the range right *  @return the value be clamped */template < class T >T clamp( T x, T min, T max ){	if ( x > max ) return max;	if ( x < min ) return min;	return x;}template < class Type>	Type pow2( const Type x ){	return x*x;}///////////////////////////////////////////////////////////////////////////////////////// deque operation template///////////////////////////////////////////////////////////////////////////////////////template < class Type >	Type getDequeValue( deque< Type > q, unsigned int i ){	if ( q.size() == 0 )		return Type();		return q[ min( i, q.size()-1 ) ];}template < class Type >	void pushDequeValue( deque< Type > &q, Type value, const unsigned int maxSize ){	//-* push the value to the front	q.push_front( value );	//-* keep the size of the deque	while( q.size() > maxSize )		q.pop_back();}template < class Type >	Type getDequeValueByTime( deque< Time > timeDeque, Time time, deque< Type > q ){	deque< Time >::iterator iter = find( timeDeque.begin(), timeDeque.end(), time );	if ( iter == timeDeque.end() )		return Type();		unsigned int i = iter - timeDeque.begin();	return getDequeValue( q, i );}/////////////////// some function of time ////////////////////Second Time2Second( const Time &t);Time Second2Time( const Second &t);Step Time2Step( const Time &t);Step Second2Step( const Second &t);Time Step2Time ( const Step &t);Second Step2Second( const Step &t);// this function convert Vector3f to Vector2f,just skip z dimVector2f projection(const Vector3f &pos3 );// this function convert Vector2f to Vector3f,let z = 0Vector3f projection(const Vector2f &pos2 );/***************************************************************//******************** CLASS RECTANGLE **************************//***************************************************************/class Rectangle{public:    f_inline                Rectangle() { }    f_inline                Rectangle(const Rectangle &inOther) : _Left(inOther.Left()), _Top(inOther.Top()), _Right(inOther.Right()), _Bottom(inOther.Bottom()) { }     f_inline                Rectangle(float inLeft, float inTop, float inRight, float inBottom)  : _Left(inLeft), _Top(inTop), _Right(inRight), _Bottom(inBottom) { }     f_inline void   Set(float inLeft, float inTop, float inRight, float inBottom)   { _Left=inLeft; _Top=inTop; _Right=inRight; _Bottom=inBottom; }     // member access    f_inline float    Left() const                    { return _Left; }    f_inline float    Right() const                   { return _Right; }    f_inline float    Top() const                     { return _Top; }    f_inline float    Bottom() const                  { return _Bottom; }    f_inline float    Width() const                   { return _Right-_Left; }    f_inline float    Height() const                  { return _Bottom-_Top; }    f_inline void   Normalize()                                                                             { if (_Right < _Left) gSwap(_Left, _Right); if (_Bottom < _Top) gSwap(_Top, _Bottom); }    f_inline void   Widen(float inDelta)                                                              { _Left-=inDelta; _Top-=inDelta; _Right+=inDelta; _Bottom+=inDelta;     }    f_inline void   Widen(float inDeltaWidth, float inDeltaHeight)              { _Right+=inDeltaWidth; _Bottom+=inDeltaHeight; }    f_inline void   Widen(float inDeltaLeft, float inDeltaTop, float inDeltaRight, float inDeltaBottom)     { _Left-=inDeltaLeft; _Top-=inDeltaTop; _Right+=inDeltaRight; _Bottom+=inDeltaBottom;           }    f_inline void   Shrink(float inDelta)                                                             { _Left+=inDelta;       _Top+=inDelta;  _Right-=inDelta; _Bottom-=inDelta;      }    f_inline void   Shrink(float inDeltaWidth, float inDeltaHeight)             { _Right-=inDeltaWidth; _Bottom-=inDeltaHeight; }    f_inline void   Shrink(float inDeltaLeft, float inDeltaTop, float inDeltaRight, float inDeltaBottom) { _Left+=inDeltaLeft; _Top+=inDeltaTop; _Right-=inDeltaRight; _Bottom-=inDeltaBottom;              }    f_inline void   Offset(float inDeltaX, float inDeltaY)                              { _Left+=inDeltaX;      _Top+=inDeltaY; _Right+=inDeltaX;       _Bottom+=inDeltaY; }    f_inline bool   Intersects(const Rectangle &b) const                                 { return !(_Left > b._Right || _Right < b._Left || _Top > b._Bottom || _Bottom < b._Top); }	     // assignment    f_inline Rectangle& operator=(const Rectangle &inOther)  { _Left=inOther.Left(); _Top=inOther.Top();     _Right=inOther.Right(); _Bottom=inOther.Bottom(); return *this; }     // comparison    f_inline bool operator==(const Rectangle &inRHS) const  { return (_Left==inRHS.Left()) && (_Top==inRHS.Top()) && (_Right==inRHS.Right()) && (_Bottom==inRHS.Bottom()); }    f_inline bool operator!=(const Rectangle &inRHS) const  { return (_Left!=inRHS.Left()) || (_Top!=inRHS.Top()) || (_Right!=inRHS.Right()) || (_Bottom!=inRHS.Bottom()); }	f_inline bool operator[](const Vector3f &p) const { return ( p.x()<_Right && p.x()>_Left && p.y()<_Top && p.y()>_Bottom); }		private:     float _Left;     float _Top;     float _Right;     float _Bottom;};/***************************************************************//******************** CLASS SECTOR *****************************//***************************************************************/class Sector