geometryr.cpp

robocup 5vs5修改板决策例程

\return the result of rotating the vector corresponding to the current
VecPosition over the given angle thus yielding a different VecPosition */
VecPosition VecPosition::Rotate( AngRad angle )
{
	// determine the polar representation of the current VecPosition
	double dMag    = this->GetMagnitude( );
	double dNewDir = this->GetDirection( ) + angle;  // add rotation angle to phi
	SetVecPosition( dMag, dNewDir, POLAR );          // convert back to Cartesian
	return ( *this );
}

/*! This method converts the coordinates of the current VecPosition (which are
represented in an global coordinate system with the origin at (0,0)) into
relative coordinates in a different coordinate system (e.g. relative to a
player). The new coordinate system is defined by the arguments to the
method. The relative coordinates are now obtained by aligning the relative
coordinate system with the global coordinate system using a translation to
Make both origins coincide followed by a rotation to align the axes.
\param origin the origin of the relative coordinate frame
\param ang the angle between the world frame and the relative frame
(reasoning from the world frame)
\return the result of converting the current global VecPosition into a
relative VecPosition */
VecPosition VecPosition::GlobalToRelative( VecPosition origin, AngRad ang )
{
	// convert global coordinates into relative coordinates by aligning relative
	// frame and world frame. First perform translation to Make origins of both
	// frames coincide. Then perform rotation to Make axes of both frames coincide
	// (use negative angle since you Rotate relative frame to world frame).
	*this -= origin;
	return ( Rotate( -ang ) );
}

/*! This method converts the coordinates of the current VecPosition (which are
represented in a relative coordinate system) into global coordinates in
the world frame (with origin at (0,0)). The relative coordinate system is
defined by the arguments to the method. The global coordinates are now
obtained by aligning the world frame with the relative frame using a
rotation to align the axes followed by a translation to Make both origins
coincide.
\param origin the origin of the relative coordinate frame
\param ang the angle between the world frame and the relative frame
(reasoning from the world frame)
\return the result of converting the current relative VecPosition into an
global VecPosition */
VecPosition VecPosition::RelativeToGlobal( VecPosition origin, AngRad ang )
{
	// convert relative coordinates into global coordinates by aligning world
	// frame and relative frame. First perform rotation to Make axes of both
	// frames coincide (use positive angle since you Rotate world frame to
	// relative frame). Then perform translation to Make origins of both frames
	// coincide.
	Rotate( ang );
	*this += origin;
	return ( *this );
}

/*! This method returns a VecPosition that lies somewhere on the vector between
the current VecPosition and a given VecPosition. The desired position is
specified by a given fraction of this vector (e.g. 0.5 means exactly in
the middle of the vector). The current VecPosition itself is left unchanged.
\param p a VecPosition which defines the vector to the current VecPosition
\param dFrac double representing the fraction of the connecting vector at
which the desired VecPosition lies.
\return the VecPosition which lies at fraction dFrac on the vector
connecting p and the current VecPosition */
VecPosition VecPosition::GetVecPositionOnLineFraction( VecPosition &p,
double      dFrac )
{
	// determine point on line that lies at fraction dFrac of whole line
	// example: this --- 0.25 ---------  p
	// formula: this + dFrac * ( p - this ) = this - dFrac * this + dFrac * p =
	//          ( 1 - dFrac ) * this + dFrac * p
	return ( ( *this ) * ( 1.0 - dFrac ) + ( p * dFrac ) );
}

/*! This method converts a polar representation of a VecPosition into a
Cartesian representation.
\param dMag a double representing the polar r-coordinate, i.e. the distance
from the point to the origin
\param ang the angle that the polar vector Makes with the x-axis, i.e. the
polar phi-coordinate
\return the result of converting the given polar representation into a
Cartesian representation thus yielding a Cartesian VecPosition */
VecPosition VecPosition::GetVecPositionFromPolar( double dMag, AngRad ang )
{
	// cos(phi) = x/r <=> x = r*cos(phi); sin(phi) = y/r <=> y = r*sin(phi)
	return ( VecPosition( dMag * cos( ang ), dMag * sin( ang ) ) );
}

/*! This method Normalizes an angle. This means that the resulting angle lies
between -PI and PI degrees.
\param angle the angle which must be Normalized
\return the result of normalizing the given angle */
AngRad VecPosition::NormalizeAngle( AngRad angle )
{
	if (fabs(angle) > 1000) angle = 0;

	while( angle > M_PI  ) angle -= 2*M_PI;
	while( angle < - M_PI  ) angle += 2*M_PI;

	return ( angle );
}

AngRad VecPosition::NormalizeAngle2PI( AngRad angle)
{
	while( angle > 2*M_PI  ) angle -= 2*M_PI;
	while( angle < 0  ) angle += 2*M_PI;

	return ( angle );
}


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

/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the
length of a geometric series given its first element, the sum of the
elements in the series and the constant ratio between the elements.
Normally: s = a + ar + ar^2 + ...  + ar^n
Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2 + .. + dFist*dRatio^n
\param dFirst first term of the series
\param dRatio ratio with which the the first term is multiplied
\param dSum the total sum of all the serie
\return the length(n in above example) of the series */
double Geometry::GetLengthGeomSeries( double dFirst, double dRatio, double dSum )
{
	// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
	// subtract: sr - s = - a + ar^n) =>  s(1-r)/a + 1 = r^n = temp
	// log r^n / n = n log r / log r = n = length
	double temp = (dSum * ( dRatio - 1 ) / dFirst) + 1;
	if( temp <= 0 )
	return -1.0;
	return log( temp ) / log( dRatio ) ;
}

/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the sum of a
geometric series given its first element, the ratio and the number of steps
in the series
Normally: s = a + ar + ar^2 + ...  + ar^n
Now: dSum = dFirst + dFirst*dRatio + ... + dFirst*dRatio^dSteps
\param dFirst first term of the series
\param dRatio ratio with which the the first term is multiplied
\param dSum the number of steps to be taken into account
\return the sum of the series */
double Geometry::GetSumGeomSeries( double dFirst, double dRatio, double dLength)
{
	// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
	// subtract: s - sr = a - ar^n) =>  s = a(1-r^n)/(1-r)
	return dFirst * ( 1 - pow( dRatio, dLength ) ) / ( 1 - dRatio ) ;
}

/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the sum of an
infinite geometric series given its first element and the constant ratio
between the elements. Note that such an infinite series will only converge
when 0<r<1.
Normally: s = a + ar + ar^2 + ar^3 + ....
Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2...
\param dFirst first term of the series
\param dRatio ratio with which the the first term is multiplied
\return the sum of the series */
double Geometry::GetSumInfGeomSeries( double dFirst, double dRatio )
{

	// s = a(1-r^n)/(1-r) with n->inf and 0<r<1 => r^n = 0
	return dFirst / ( 1 - dRatio );
}

/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the first element
of a geometric series given its element, the ratio and the number of steps
in the series
Normally: s = a + ar + ar^2 + ...  + ar^n
Now: dSum = dFirst + dFirst*dRatio + ... + dFirst*dRatio^dSteps
\param dSum sum of the series
\param dRatio ratio with which the the first term is multiplied
\param dSum the number of steps to be taken into account
\return the first element (a) of a serie */
double Geometry::GetFirstGeomSeries( double dSum, double dRatio, double dLength)
{
	// s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
	// subtract: s - sr = a - ar^n) =>  s = a(1-r^n)/(1-r) => a = s*(1-r)/(1-r^n)
	return dSum *  ( 1 - dRatio )/( 1 - pow( dRatio, dLength ) ) ;
}

/*! A geometric series is one in which there is a constant ratio between each
element and the one preceding it. This method determines the first element
of an infinite geometric series given its first element and the constant
ratio between the elements. Note that such an infinite series will only
converge when 0<r<1.
Normally: s = a + ar + ar^2 + ar^3 + ....
Now: dSum = dFirst + dFirst*dRatio + dFirst*dRatio^2...
\param dSum sum of the series
\param dRatio ratio with which the the first term is multiplied
\return the first term of the series */
double Geometry::GetFirstInfGeomSeries( double dSum, double dRatio )
{

	// s = a(1-r^n)/(1-r) with r->inf and 0<r<1 => r^n = 0 => a = s ( 1 - r)
	return dSum * ( 1 - dRatio );
}

/*! This method performs the abc formula (Pythagoras' Theorem) on the given
parameters and puts the result in *s1 en *s2. It returns the number of
found coordinates.
\param a a parameter in abc formula
\param b b parameter in abc formula
\param c c parameter in abc formula
\param *s1 first result of abc formula
\param *s2 second result of abc formula
\return number of found x-coordinates */
int Geometry::AbcFormula(double a, double b, double c, double *s1, double *s2)
{
	double dDiscr = b*b - 4*a*c;       // discriminant is b^2 - 4*a*c
	if (fabs(dDiscr) < EPS )       // if discriminant = 0
	{
		*s1 = -b / (2 * a);              //  only one solution
		return 1;
	}
	else if (dDiscr < 0)               // if discriminant < 0
	return 0;                        //  no solutions
	else                               // if discriminant > 0
	{
		dDiscr = sqrt(dDiscr);           //  two solutions
		*s1 = (-b + dDiscr ) / (2 * a);
		*s2 = (-b - dDiscr ) / (2 * a);
		return 2;
	}
}

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

/*! This is the constructor of a circle.
\param pos first point that defines the center of circle
\param dR the radius of the circle
\return circle with pos as center and radius as radius*/
Circle::Circle( VecPosition pos, double dR )
{
	SetCircle( pos, dR );
}

/*! This is the constructor of a circle which initializes a circle with a
radius of zero. */
Circle::Circle( )
{
	SetCircle( VecPosition(-1000.0,-1000.0), 0);
}


/*! This method Sets the values of the circle.
\param pos new center of the circle
\param dR new radius of the circle
( > 0 )
\return bool indicating whether radius was Set */
bool Circle::SetCircle( VecPosition pos, double dR )
{
	SetCenter( pos );
	return SetRadius( dR  );
}
/*! This method Sets the radius of the circle.
\param dR new radius of the circle ( > 0 )
\return bool indicating whether radius was Set */
bool Circle::SetRadius( double dR )
{
	if( dR > 0 )
	{
		m_dRadius = dR;
		return true;
	}
	else
	{
		m_dRadius = 0.0;
		return false;
	}
}

/*! This method returns the radius of the circle.
\return radius of the circle */
double Circle::GetRadius()
{
	return m_dRadius;
}

/*! This method Sets the center of the circle.
\param pos new center of the circle
\return bool indicating whether center was Set */
bool Circle::SetCenter( VecPosition pos )
{
	m_posCenter = pos;
	return true;
}

/*! This method returns the center of the circle.
\return center of the circle */
VecPosition Circle::GetCenter()
{
	return m_posCenter;
}

/*! This method returns the circumference of the circle.
\return circumference of the circle */
double Circle::GetCircumference()
{
	return 2.0*M_PI*GetRadius();
}

/*! This method returns the area inside the circle.
\return area inside the circle */
double Circle::GetArea()
{
	return M_PI*GetRadius()*GetRadius();
}

/*! This method returns a boolean that indicates whether 'pos' is located inside
the circle.
\param pos position of which should be checked whether it is located in the
circle
\return bool indicating whether pos lies inside the circle */
bool Circle::IsInside( VecPosition pos )
{
	return m_posCenter.GetDistanceTo( pos ) < GetRadius() ;
}
/*! This method returns the two possible intersection points between two
circles. This method returns the number of solutions that were found.
\param c circle with which intersection should be found
\param p1 will be filled with first solution
\param p2 will be filled with second solution
\return number of solutions. */
int Circle::GetIntersectionPoints( Circle c, VecPosition *p1, VecPosition *p2)
{
	double x0, y0, r0;
	double x1, y1, r1;

	x0 = GetCenter( ).GetX();
	y0 = GetCenter( ).GetY();
	r0 = GetRadius( );
	x1 = c.GetCenter( ).GetX();
	y1 = c.GetCenter( ).GetY();
	r1 = c.GetRadius( );

	double      d, dx, dy, h, a, x, y, p2_x, p2_y;

	// first Calculate distance between two centers circles P0 and P1.
	dx = x1 - x0;
	dy = y1 - y0;
	d = sqrt(dx*dx + dy*dy);

	// Normalize differences
	dx /= d; dy /= d;

	// a is distance between p0 and point that is the intersection point P2
	// that intersects P0-P1 and the line that crosses the two intersection
	// points P3 and P4.
	// Define two triangles: P0,P2,P3 and P1,P2,P3.
	// with distances a, h, r0 and b, h, r1 with d = a + b
	// We know a^2 + h^2 = r0^2 and b^2 + h^2 = r1^2 which then gives
	// a^2 + r1^2 - b^2 = r0^2 with d = a + b ==> a^2 + r1^2 - (d-a)^2 = r0^2
	// ==> r0^2 + d^2 - r1^2 / 2*d
	a = (r0*r0 + d*d - r1*r1) / (2.0 * d);

	// h is then a^2 + h^2 = r0^2 ==> h = sqrt( r0^2 - a^2 )
	double      arg = r0*r0 - a*a;
	h = (arg > 0.0) ? sqrt(arg) : 0.0;

	// First Calculate P2
	p2_x = x0 + a * dx;
	p2_y = y0 + a * dy;

	// and finally the two intersection points
	x =  p2_x - h * dy;
	y =  p2_y + h * dx;
	p1->SetVecPosition( x, y );
	x =  p2_x + h * dy;
	y =  p2_y - h * dx;
	p2->SetVecPosition( x, y );

	return (arg < 0.0) ? 0 : ((arg == 0.0 ) ? 1 :  2);
}

/*! This method returns the size of the intersection area of two circles.
\param c circle with which intersection should be determined
\return size of the intersection areACT-> */
double Circle::GetIntersectionArea( Circle c )
{
	VecPosition pos1, pos2, pos3;
	double d, h, dArea;
	AngDeg ang;

	d = GetCenter().GetDistanceTo( c.GetCenter() ); // dist between two centers
	if( d > c.GetRadius() + GetRadius() )           // larger than sum radii
	return 0.0;                                   // circles do not intersect
	if( d <= fabs(c.GetRadius() - GetRadius() ) )   // one totally in the other
	{
		double dR = Maths::Min( c.GetRadius(), GetRadius() );// return area smallest circle
		return M_PI*dR*dR;
	}