geometry.h

东南大学最新的校内比赛的源代码

/*****************************************************************************************
 *                                      SEU-3D
 *                     -------------------------------------------------
 * Copyright (c) 2005, Yuan XU<[e-mail]>,Chang'e SHI<[e-mail]>
 * Copyright (c) 2006, Yuan XU<[e-mail]>,Chunlu JIANG<[e-mail]>
 * 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 __cplusplus
extern "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 radians
AngDeg 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 functions
bool   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 Vector3f
Vector3f 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 dim
Vector2f projection(const Vector3f &pos3 );

// this function convert Vector2f to Vector3f,let z = 0
Vector3f 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