geometry.cpp

一个Ｃ编写的足球机器人比赛程序

    // 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 area. */
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 = min( c.getRadius(), getRadius() );// return area smallest circ
		return M_PI*dR*dR;
	}
	
	int iNrSol = getIntersectionPoints( c, &pos1, &pos2 );
	if( iNrSol != 2 )
		return 0.0;
	
	// the intersection area of two circles can be divided into two segments:
	// left and right of the line between the two intersection points p1 and p2.
	// The outside area of each segment can be calculated by taking the part
	// of the circle pie excluding the triangle from the center to the
	// two intersection points.
	// The pie equals pi*r^2 * rad(2*ang) / 2*pi = 0.5*rad(2*ang)*r^2 with ang
	// the angle between the center c of the circle and one of the two
	// intersection points. Thus the angle between c and p1 and c and p3 where
	// p3 is the point that lies halfway between p1 and p2.
	// This can be calculated using ang = asin( d / r ) with d the distance
	// between p1 and p3 and r the radius of the circle.
	// The area of the triangle is 2*0.5*h*d.
	
	pos3 = pos1.getVecPositionOnLineFraction( pos2, 0.5 );
	d = pos1.getDistanceTo( pos3 );
	h = pos3.getDistanceTo( getCenter() );
	ang = asin( d / getRadius() );
	
	dArea = ang*getRadius()*getRadius();
	dArea = dArea - d*h;
	
	// and now for the other segment the same story
	h = pos3.getDistanceTo( c.getCenter() );
	ang = asin( d / c.getRadius() );
	dArea = dArea + ang*c.getRadius()*c.getRadius();
	dArea = dArea - d*h;
	
	return dArea;
}


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

/*! This constructor creates a line by given the three coefficents of the line.
A line is specified by the formula ay + bx + c = 0.
\param dA a coefficients of the line
\param dB b coefficients of the line
\param dC c coefficients of the line */
Line::Line( double dA, double dB, double dC )
{
	m_a = dA;
	m_b = dB;
	m_c = dC;
}


/*! This method returns the intersection point between the current Line and
the specified line.
\param line line with which the intersection should be calculated.
\return VecPosition position that is the intersection point. */
VecPosition Line::getIntersection( Line line )
{
	VecPosition pos;
	double x, y;
	if(m_b == 0 || line.getBCoefficient() == 0)
	{
		if(m_b == line.getBCoefficient())
		{
			return pos;  // lines are parallel, no intersection
		}
		else if(m_b == 0)
		{
			y = - m_c / m_a;
			x = line.getXGivenY(y);
		}
		else
		{
			y = - line.getCCoefficient() / line.getACoefficient();
			x = getXGivenY(y);
		}
		return VecPosition( x, y );
	}
	if( ( m_a / m_b ) ==  (line.getACoefficient() / line.getBCoefficient() ))
		return pos; // lines are parallel, no intersection
	if( m_a == 0 )            // bx + c = 0 and a2*y + b2*x + c2 = 0 ==> x = -c/b
	{                          // calculate x using the current line
		x = -m_c/m_b;                // and calculate the y using the second line
		y = line.getYGivenX(x);
	}
	else if( line.getACoefficient() == 0 )
	{                         // ay + bx + c = 0 and b2*x + c2 = 0 ==> x = -c2/b2
		x = -line.getCCoefficient()/line.getBCoefficient(); // calculate x using
		y = getYGivenX(x);       // 2nd line and calculate y using current line
	}
	// ay + bx + c = 0 and a2y + b2*x + c2 = 0
	// y = (-b2/a2)x - c2/a2
	// bx = -a*y - c =>  bx = -a*(-b2/a2)x -a*(-c2/a2) - c ==>
	// ==> a2*bx = a*b2*x + a*c2 - a2*c ==> x = (a*c2 - a2*c)/(a2*b - a*b2)
	// calculate x using the above formula and the y using the current line
	else
	{
		x = (m_a*line.getCCoefficient() - line.getACoefficient()*m_c)/
			(line.getACoefficient()*m_b - m_a*line.getBCoefficient());
		y = getYGivenX(x);
	}
	
	return VecPosition( x, y );
}


/*! This method calculates the intersection points between the current line
and the circle specified with as center 'posCenter' and radius 'dRadius'.
The number of solutions are returned and the corresponding points are put
in the third and fourth argument of the method
\param c circle with which intersection points should be found
\param posSolution1 first intersection (if any)
\param posSolution2 second intersection (if any) */
int Line::getCircleIntersectionPoints( Circle circle,
									  VecPosition *posSolution1, VecPosition *posSolution2 )
{
	int    iSol;
	double dSol1=0, dSol2=0;
	double h = circle.getCenter().getX();
	double k = circle.getCenter().getY();
	
	// line:   x = -c/b (if a = 0)
	// circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y
	// fill in:(-c/b-h)^2 + y^2 -2ky + k^2 - r^2 = 0
	//         y^2 -2ky + (-c/b-h)^2 + k^2 - r^2 = 0
	// and determine solutions for y using abc-formula
	if( fabs(m_a) < EPSILON )
	{
		iSol = Geometry::abcFormula( 1, -2*k, ((-m_c/m_b) - h)*((-m_c/m_b) - h)
			+ k*k - circle.getRadius()*circle.getRadius(), &dSol1, &dSol2);
		posSolution1->setVecPosition( (-m_c/m_b), dSol1 );
		posSolution2->setVecPosition( (-m_c/m_b), dSol2 );
		return iSol;
	}
	
	// ay + bx + c = 0 => y = -b/a x - c/a, with da = -b/a and db = -c/a
	// circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y
	// fill in:x^2 -2hx + h^2 + (da*x-db)^2 -2k(da*x-db) + k^2 - r^2 = 0
	//         x^2 -2hx + h^2 + da^2*x^2 + 2da*db*x + db^2 -2k*da*x -2k*db
	//                                                         + k^2 - r^2 = 0
	//       (1+da^2)*x^2 + 2(da*db-h-k*da)*x + h2 + db^2  -2k*db + k^2 - r^2 = 0
	// and determine solutions for x using abc-formula
	// fill in x in original line equation to get y coordinate
	double da = -m_b/m_a;
	double db = -m_c/m_a;
	
	double dA = 1 + da*da;
	double dB = 2*( da*db - h - k*da );
	double dC = h*h + db*db-2*k*db + k*k - circle.getRadius()*circle.getRadius();
	
	iSol = Geometry::abcFormula( dA, dB, dC, &dSol1, &dSol2 );
	
	posSolution1->setVecPosition( dSol1, da*dSol1 + db );
	posSolution2->setVecPosition( dSol2, da*dSol2 + db );
	return iSol;
	
}

/*! This method returns the tangent line to a VecPosition. This is the line
between the specified position and the closest point on the line to this
position.
\param pos VecPosition point with which tangent line is calculated.
\return Line line tangent to this position */
Line Line::getTangentLine( VecPosition pos )
{
	// ay + bx + c = 0 -> y = (-b/a)x + (-c/a)
	// tangent: y = (a/b)*x + C1 -> by - ax + C2 = 0 => C2 = ax - by
	// with pos.y = y, pos.x = x
	return Line( m_b, -m_a, m_a*pos.getX() - m_b*pos.getY() );
}

/*! This method returns the closest point on a line to a given position.
\param pos point to which closest point should be determined
\return VecPosition closest point on line to 'pos'. */
VecPosition Line::getPointOnLineClosestTo( VecPosition pos )
{
	Line l2 = getTangentLine( pos );  // get tangent line
	return getIntersection( l2 );     // and intersection between the two lines
}

/*! This method returns the distance between a specified position and the
closest point on the given line.
\param pos position to which distance should be calculated
\return double indicating the distance to the line. */
double Line::getDistanceWithPoint( VecPosition pos )
{
	return pos.getDistanceTo( getPointOnLineClosestTo( pos ) );
}

/*! This method determines whether the projection of a point on the
current line lies between two other points ('point1' and 'point2')
that lie on the same line.

  \param pos point of which projection is checked.
  \param point1 first point on line
  \param point2 second point on line
\return true when projection of 'pos' lies between 'point1' and 'point2'.*/
bool Line::isInBetween( VecPosition pos, VecPosition point1,VecPosition point2)
{
	pos          = getPointOnLineClosestTo( pos ); // get closest point
	double dDist = point1.getDistanceTo( point2 ); // get distance between 2 pos
	
	// if the distance from both points to the projection is smaller than this
	// dist, the pos lies in between.
	return pos.getDistanceTo( point1 ) <= dDist &&
		pos.getDistanceTo( point2 ) <= dDist;
}

/*! This method calculates the y coordinate given the x coordinate
\param x coordinate
\return y coordinate on this line */
double Line::getYGivenX( double x )
{
	if( m_a == 0 )
	{
		return 0;
	}
	// ay + bx + c = 0 ==> ay = -(b*x + c)/a
	return -(m_b*x+m_c)/m_a;
}

/*! This method calculates the x coordinate given the x coordinate
\param y coordinate
\return x coordinate on this line */
double Line::getXGivenY( double y )
{
	if( m_b == 0 )
	{
		cerr << "(Line::getXGivenY) Cannot calculate X coordinate\n" ;
		return 0;
	}
	// ay + bx + c = 0 ==> bx = -(a*y + c)/a
	return -(m_a*y+m_c)/m_b;
}

/*! This method creates a line given two points.
\param pos1 first point
\param pos2 second point
\return line that passes through the two specified points. */
Line Line::makeLineFromTwoPoints( VecPosition pos1, VecPosition pos2 )
{
	// 1*y + bx + c = 0 => y = -bx - c
	// with -b the direction coefficient (or slope)
	// and c = - y - bx
	double dA, dB, dC;
	double dTemp = pos2.getX() - pos1.getX(); // determine the slope
	if( fabs(dTemp) < EPSILON )
	{
		// ay + bx + c = 0 with vertical slope=> a = 0, b = 1
		dA = 0.0;
		dB = 1.0;
	}
	else
	{
		// y = (-b)x -c with -b the slope of the line
		dA = 1.0;
		dB = -(pos2.getY() - pos1.getY())/dTemp;
	}
	// ay + bx + c = 0 ==> c = -a*y - b*x
	dC =  - dA*pos2.getY()  - dB * pos2.getX();
	return Line( dA, dB, dC );
}

/*! This method creates a line given a position and an angle.
\param vec position through which the line passes
\param angle direction of the line.
\return line that goes through position 'vec' with angle 'angle'. */
Line Line::makeLineFromPositionAndAngle( VecPosition vec, AngDeg angle )
{
	// calculate point somewhat further in direction 'angle' and make
	// line from these two points.
	return makeLineFromTwoPoints( vec, vec+VecPosition(1,angle,POLAR));
}

/*! This method returns the a coefficient from the line ay + bx + c = 0.
\return a coefficient of the line. */
double Line::getACoefficient() const
{
	return m_a;
}

/*! This method returns the b coefficient from the line ay + bx + c = 0.
\return b coefficient of the line. */
double Line::getBCoefficient() const
{
	return m_b;
}

/*! This method returns the c coefficient from the line ay + bx + c = 0.
\return c coefficient of the line. */
double Line::getCCoefficient() const
{
	return m_c;
}

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

/*! This is the constructor of a Rectangle. Two points will be given. The
order does not matter as long as two opposite points are given (left
top and right bottom or right top and left bottom).
\param pos first point that defines corner of rectangle
\param pos2 second point that defines other corner of rectangle
\return rectangle with 'pos' and 'pos2' as opposite corners. */
Rect::Rect( VecPosition pos, VecPosition pos2 )
{
	setRectanglePoints( pos, pos2 );
}

/*! This method sets the upper left and right bottom point of the current
rectangle.
\param pos first point that defines corner of rectangle
\param pos2 second point that defines other corner of rectangle */
void Rect::setRectanglePoints( VecPosition pos1, VecPosition pos2 )
{
	m_posLeftTop.setX    ( max( pos1.getX(), pos2.getX() ) );
	m_posLeftTop.setY    ( min( pos1.getY(), pos2.getY() ) );
	m_posRightBottom.setX( min( pos1.getX(), pos2.getX() ) );
	m_posRightBottom.setY( max( pos1.getY(), pos2.getY() ) );
}

/*! This method determines whether the given position lies inside the current
rectangle.
\param pos position which is checked whether it lies in rectangle
\return true when 'pos' lies in the rectangle, false otherwise */
bool Rect::isInside( VecPosition pos )
{
	return pos.isBetweenX( m_posRightBottom.getX(), m_posLeftTop.getX() ) &&
		pos.isBetweenY( m_posLeftTop.getY(),     m_posRightBottom.getY() );
	
}

/*! This method sets the top left position of the rectangle
\param pos new top left position of the rectangle
\return true when update was successful */
bool Rect::setPosLeftTop( VecPosition pos )
{
	m_posLeftTop = pos;
	return true;
}

/*! This method returns the top left position of the rectangle
\return top left position of the rectangle */
VecPosition Rect::getPosLeftTop(  )
{
	return m_posLeftTop;
}

/*! This method sets the right bottom position of the rectangle
\param pos new right bottom position of the rectangle
\return true when update was successful */
bool Rect::setPosRightBottom( VecPosition pos )
{
	m_posRightBottom = pos;
	return true;
}

/*! This method returns the right bottom position of the rectangle
\return top right bottom of the rectangle */
VecPosition Rect::getPosRightBottom(  )
{
	return m_posRightBottom;
}