geometry.cpp

仿真机器人的经典代码

//the methods used here all refer to nature coordinate!
#include "stdafx.h"
#include "globe.h"
#include "geometry.h"
#include "StrategySystem.h"

/*********** chen ning *******************/
int CalcuFormat(POINT P2, POINT P1, Formulation *Param)
{//----------- calculate the line P1 and P2's formulation according to
	//the format aY = bX + c, a=x1-x2,b=y1-y2, c=x1*y2-x2*y1.
	// return 1 if (x2-x1) > 0, return -1 if (x2-x1) < 0
	// else return 0. the returned value can be used to determine 
	// the line's normal direction.
	//nextstep:aY + bX + c = 0, a = x1-x2,b=y2-y1,c=x2*y1-x1*y2. 
	Param->a = P2.x - P1.x;
	Param->b = P2.y - P2.y;
	Param->c = P1.y*P2.x - P2.y*P1.x;
	if(Param->a > 0)
		return 1;
	if(Param->a < 0)
		return -1;
	return 0;
};
int IsInVectorRegion(POINT A, POINT P1, POINT P2)
{//--- calculate to see if point A is in the normal area of line P1,P2.
	// the line's direction is from P1 to P2. the line's normal direction
	// is decided by the counter-clockwise rule.
	// return 1 if is, return -1 if not, otherwise return 0.
	// use nature coordinate!
	Formulation param;
	int k, i = CalcuFormat(P2, P1, &param);
	double y;
	if((param.a == 0) && (param.b == 0))
		return -2;		//illegal data
	if(!i)
	{
		i = param.b>0 ? 1 : -1;
		if(-param.c/param.b > A.x)
			k = 1;
		else
			k = (A.x - (int)(param.c/param.b))==0 ? 0 : 1;
		return i * k;
	};
	if(param.b == 0)
	{
		if(param.c/param.a < A.y)
			k = 1;
		else
			k = ((int)(param.c/param.a) - A.y)==0 ? 0 : 1;
			return i * k;
	};
	y = (param.b * A.x + param.c)/param.a;
	if(A.y > (int)y)
		k = 1;
	else
		k = A.y == (int)y ? 0 : 1;
	return i * k;
}
int IsInObject(POINT A, POINT *points, int number)
{
	//calculate to see if point A is in the object decided by the points.
	//return 1 if is, return -1 if not, return 0 if A is right on the edge.
	if(number<3)
		return -2;
	int i, cross=0, r;
	for(i=0; i<number-1;i++)
	{
		if(r = IsInVectorRegion(A, points[i], points[i+1]) < 0)
			return -1;
		if (r==0) 
			cross++;
	}
	if(r = IsInVectorRegion(A, points[i], points[0]) < 0)
		return -1;
	if (r==0) 
		cross++;
    if (cross)
		return 0;
	return 1;
}
int IsInObject(POINT A, POINT O, int radian)
{
	int r = (O.x - A.x) * (O.x - A.x) + (O.y - A.y) * (O.y - A.y);
	if((r = r - radian * radian) > 0)
		return -1;
	if(r==0)
		return 0;
	return 1;
}
int gmFormulate(POINT P1, POINT P2, Formulation *FormuParam)
{//---------aX + bY + c = 0, b = x1-x2,a=y2-y1,c=x2*y1-x1*y2
	FormuParam->a = P2.y - P1.y;
	FormuParam->b = P1.x - P2.x;
	FormuParam->c = P2.x*P1.y - P1.x*P2.y;
	return 0;
}
int gmIsInVectorRegion(POINT A, POINT P1, POINT P2)
{
	if((P1.x == P2.x)&&(P1.y==P2.y))
		return -2;//same points, no direction
	Formulation lineformu;
	double y;
	gmFormulate(P1, P2, &lineformu);
	if(lineformu.b == 0)
	{
		if(A.x == (-lineformu.c/lineformu.a))
			return 0;
		if((A.x+lineformu.c/lineformu.a)*(P2.y -P1.y)>0)
			return -1;
		else
			return 1;
	}
	else
		y = -(lineformu.a*A.x + lineformu.c)/lineformu.b;
	if(y==A.y)
		return 0;
	if(lineformu.a == 0)
	{
		if((A.y + lineformu.c/lineformu.b)*(P2.x - P1.x)>0)
			return 1;
		else
			return -1;
	}
	if((y-A.y)*(P2.y-P1.y)*(P2.x-P1.x)>0)
		return -1;
	else
		return 1;
	return 2;
}

int StdLineForm(dbPOINT point, double angle, LINEFORMULATION *Result)
{
//点斜式直线求出标准方程
	double cn_pi = acos(-1);
	if((angle - cn_pi/2) == 0)
	{
		Result->a = 1;
		Result->b = 0;
		Result->c = -point.x;
	}
	else
	{
		Result->a = -tan(angle);
		Result->b = 1;
		Result->c = tan(angle)*point.x - point.y;
	}
	return 1;
}

int StdLineForm(dbPOINT P1, dbPOINT P2, Formulation *FormuParam)
{//---------aX + bY + c = 0, b = x1-x2,a=y2-y1,c=x2*y1-x1*y2
	FormuParam->a = P2.y - P1.y;
	FormuParam->b = P1.x - P2.x;
	FormuParam->c = P2.x*P1.y - P1.x*P2.y;
	return 0;
}
int StdLineForm(POINT P1, POINT P2, Formulation *FormuParam)
{//---------aX + bY + c = 0, b = x1-x2,a=y2-y1,c=x2*y1-x1*y2
	FormuParam->a = P2.y - P1.y;
	FormuParam->b = P1.x - P2.x;
	FormuParam->c = P2.x*P1.y - P1.x*P2.y;
	return 0;
}
int cn_VectorRegion(dbPOINT A, dbPOINT P1, dbPOINT P2)
{
	if((P1.x == P2.x)&&(P1.y==P2.y))
	return -2;//same points, no direction
	double u, cn_pi = acos(-1);
	double theta = atan2(P2.y-P1.y, P2.x-P1.x);
	if((theta == cn_pi/2) || (theta == -cn_pi/2))
	{
		u = (A.x - P1.x) * theta;
		if(u>0)
			return -1;
		else
			if(u<0)
				return 1;
			return 0;
	};
	if(((theta>cn_pi/2)&&(theta<=cn_pi))||((theta<0)&&(theta>-cn_pi/2)))
		theta = -theta;
	Formulation lineformu;
	StdLineForm(P1, P2, &lineformu);
	u = -(lineformu.a*A.x+lineformu.c)/lineformu.b;
	if((A.y - u)*theta>0)
		return 1;
	else
		if((A.y - u)*theta<0)
			return -1;
	return 0;
}
int cn_VectorRegion(POINT A, POINT P1, POINT P2)
{//to see if point A is in the vector region of P1、P2
	if((P1.x == P2.x)&&(P1.y==P2.y))
	return -2;//same points, no direction
	double u, cn_pi = acos(-1);
	double theta = atan2(P2.y-P1.y, P2.x-P1.x);
	if(P2.x == P1.x)
	//if((theta == cn_pi/2) || (theta == -cn_pi/2))
	{
		u = (A.x - P1.x) * theta;
		if(u>0)
			return -1;
		else
			if(u<0)
				return 1;
			return 0;
	};
	if(P2.y == P1.y)
	{
		if((P2.x - P1.x)*(P1.y - A.y) > 0)
			return -1;
		else
			if((P2.x - P1.x)*(P1.y - A.y) == 0)
				return 0;
		return 1;
	}
	if(((theta>cn_pi/2)&&(theta<=cn_pi))||((theta<0)&&(theta>-cn_pi/2)))
		theta = -theta;
	Formulation lineformu;
	StdLineForm(P1, P2, &lineformu);
	u = -(lineformu.a*A.x+lineformu.c)/lineformu.b;
	if((A.y - u)*theta>0)
		return 1;
	else
		if((A.y - u)*theta<0)
			return -1;
	return 0;
}

int cn_2LinesCrossPoint(Formulation *Line1, Formulation *Line2, dbPOINT *Result)
{//get Line1 and Line2's cross point, return -1 if no cross point, otherwise return 1.
	double dt = Line1->a*Line2->b - Line2->a*Line1->b;
	if( dt == 0)
		return -1;
	if(Result!=NULL)
	{
		Result->x = (Line1->b*Line2->c - Line2->b*Line1->c)/dt;
		Result->y = (Line1->c*Line2->a - Line2->c*Line1->a)/dt;
	}
	return 1;
}

int cn_2LinesCrossPoint(dbPOINT A1, dbPOINT A2, dbPOINT B1, dbPOINT B2, dbPOINT *Result)
{
	Formulation lineA, lineB;
	StdLineForm(A1, A2, &lineA);
	StdLineForm(B1, B2, &lineB);
	return cn_2LinesCrossPoint(&lineA, &lineB, Result);
}

int cn_LineCircleCross(LINEFORMULATION *pLine, CIRCLEFORMULATION *pCircle, dbPOINT *Point1, dbPOINT *Point2)
{//get line and circle's cross points, return -1 if no cross, 0 if there is one cross
//point, return 1 otherwise.
	double delta;
	delta = (pow(pLine->a,2) + pow(pLine->b,2))*pow(pCircle->r,2) - 
		pow(pLine->a*pCircle->x + pLine->b*pCircle->y + pLine->c, 2);
	if(delta<0)
		return -1;//no cross point
	delta = sqrt(delta);
	Point1->x = ((pow(pLine->b,2)*pCircle->x - pLine->a*pLine->b*pCircle->y - 
		pLine->a*pLine->c) + pLine->b*delta)/(pow(pLine->a,2) + pow(pLine->b,2));
	Point1->y = ((pow(pLine->a,2)*pCircle->y - pLine->a*pLine->b*pCircle->x - 
		pLine->b*pLine->c) - pLine->a*delta)/(pow(pLine->a,2) + pow(pLine->b,2));
	Point2->x = ((pow(pLine->b,2)*pCircle->x - pLine->a*pLine->b*pCircle->y - 
		pLine->a*pLine->c) - pLine->b*delta)/(pow(pLine->a,2) + pow(pLine->b,2));
	Point2->y = ((pow(pLine->a,2)*pCircle->y - pLine->a*pLine->b*pCircle->x - 
		pLine->b*pLine->c) + pLine->a*delta)/(pow(pLine->a,2) + pow(pLine->b,2));
	if(delta==0)
		return 0;
	return 1;
}

int cn_LineCircleCross(dbPOINT A, dbPOINT B, CIRCLEFORMULATION *pCircle, dbPOINT *Point1, dbPOINT *Point2)
{
	LINEFORMULATION line;
	StdLineForm(A, B, &line);
	return cn_LineCircleCross(&line, pCircle, Point1, Point2);
}

int cn_2CirclesCross(CIRCLEFORMULATION *pCircle1, CIRCLEFORMULATION *pCircle2, dbPOINT *Point1, dbPOINT *Point2)
{//get 2 circle's cross point, return -1 if no cross point, otherwise return 1
	LINEFORMULATION line;
	line.a = 2.0*(pCircle2->x - pCircle1->x);
	line.b = 2.0*(pCircle2->y - pCircle1->y);
	line.c = pow(pCircle2->r,2) - pow(pCircle1->r,2) - 
		(pow(pCircle2->x,2) - pow(pCircle1->x,2)) - (pow(pCircle2->y,2) - pow(pCircle1->y,2));
	return cn_LineCircleCross(&line, pCircle1, Point1, Point2);
}

double cn_LineAngle(LINEFORMULATION *pLine)
{
	double theta = atan2(pLine->a, -pLine->b);
	return cn_AngleTrimPI(theta);
}

double cn_LineAngle(POINT A, POINT B)///////
{// Calculate the angle between line AB and the x-axis, the result is between 0-2*pi,
// and the line's direction is from A to B.
	double angle=atan2(B.y-A.y, B.x-A.x);
	double cn_pi = acos(-1.0);
	if(angle<0)
		angle += 2*cn_pi;
	return angle;
}

int cn_IsPointInSegment(dbPOINT Pt, dbPOINT A, dbPOINT B)
{
	double d1, d2;
	d1 = cn_2PointsDist(Pt, A);
	d2 = cn_2PointsDist(Pt, B);
	if(fabs(d1 + d2 - cn_2PointsDist(A, B)) < 0.0000000001)
		return 1;
	return -1;
}

int cn_IsPointInArc(dbPOINT Pt, CIRCLEFORMULATION *pCircleFormulation, double sTheta, double dTheta)
{// in this section the delta arc's direction  obeys the anticlockwise!
	double d = pow(Pt.x - pCircleFormulation->x, 2) + pow(Pt.y - pCircleFormulation->y, 2);
	if(fabs(d - pCircleFormulation->r) >= 0.0000000001)
		return -1;
	double theta1, theta2, theta;
	dbPOINT o;
	o.x = pCircleFormulation->x;
	o.y = pCircleFormulation->y;
	theta = sTheta + dTheta;
	d = cn_LineAngle(o, Pt);
	theta1 = cn_AngleTrim2PI(fabs(d - sTheta));
	theta2 = cn_AngleTrim2PI(fabs(theta - d));
	if(fabs(theta1 + theta2 - fabs(dTheta))<0.0000000001)
		return 1;
	return -1;
}

double cn_LineAngle(dbPOINT A, dbPOINT B)
{// Calculate the angle of line AB and the x-axis, the result is between 0-2*pi
// and the line's direction is from A to B.
	double cn_pi = acos(-1.0);
	double angle=atan2(B.y-A.y, B.x-A.x);
	if(angle<0)
		angle += 2*cn_pi;
	return angle;
}

int cn_PointPerpendLine(dbPOINT Point, LINEFORMULATION *pLine, LINEFORMULATION *pResult, dbPOINT *pPointC)
{
	double angle = cn_LineAngle(pLine);
	angle += pi/2;
	StdLineForm(Point, angle, pResult);
	cn_2LinesCrossPoint(pLine, pResult, pPointC);