geometry.cpp

仿真机器人的经典代码

	return 1;
}

int cn_PointPerpendLine(dbPOINT Point, dbPOINT A, dbPOINT B, LINEFORMULATION *pResult, dbPOINT *pPointCross)
{//过一点垂直于一条直线的方程和交点
	LINEFORMULATION line;
	StdLineForm(A, B, &line);
	return cn_PointPerpendLine(Point, &line, pResult, pPointCross);
}

int cn_PointInLineSegment(dbPOINT A, dbPOINT B, double Distance, dbPOINT *Result)
{//obtain the point which distance from A is Distance and location is in linesegment AB.
	double angle = cn_LineAngle( A, B);
	Result->x = A.x + Distance * cos(angle);
	Result->y = A.x + Distance * sin(angle);
	return 0;
}
int cn_PointDistanceInLine(dbPOINT A, dbPOINT B, double Distance, dbPOINT *Result1, dbPOINT *Result2)
{//obtain the 2 points which distance from A is Distance and location is in line AB.
	double angle = cn_LineAngle( A, B);
	Result1->x = A.x + Distance * cos(angle);
	Result1->y = A.x + Distance * sin(angle);
	Result2->x = A.x - Distance * cos(angle);
	Result2->y = A.x - Distance * sin(angle);
	return 0;
}

int cn_PointSlopeLine(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 0;
}


int cn_DistanceLines(LINEFORMULATION *pRefLine, double Distance, LINEFORMULATION *Result1, LINEFORMULATION *Result2)
{//get the 2 lines that parallel the given line with the given distance
	*Result1 = *pRefLine;
	*Result2 = *pRefLine;
	if(pRefLine->a==0)
	{
		Result1->c = pRefLine->c + pRefLine->b * Distance;
		Result2->c = pRefLine->c - pRefLine->b * Distance;
		return 1;
	}
	double theta = cn_LineAngle(pRefLine);
	Result1->c = pRefLine->c + pRefLine->a * Distance / sin(theta);
	Result2->c = pRefLine->c - pRefLine->a * Distance / sin(theta);
	return 1;
}

int cn_2SectionsCross(POINT A, POINT B, POINT C, POINT D, POINT* P)
{//to see if the 2 line segments, AB and CD, cross. If does, the returned 
// value is 1, the returned value is -1 if not, return 0 if the crosspoint
// is in one of the 2 segments rightaway.
	int i = cn_VectorRegion(A, C, D);
	int j = cn_VectorRegion(B, C, D);
	if(i*j==1)
		return -1;//same side
	int m = cn_VectorRegion(C, A, B);
	int n = cn_VectorRegion(D, A, B);
	if(m*n==1)
		return -1;//same side, no cross point
	if(P!=NULL)
	{
		Formulation lineformu1;
		StdLineForm(A, B, &lineformu1);
		Formulation lineformu2;
		StdLineForm(C, D, &lineformu2);
		P->x = (int)((lineformu1.b*lineformu2.c - lineformu2.b*lineformu1.c)
					/(lineformu1.a*lineformu2.b - lineformu2.a*lineformu1.b));
		P->y = (int)((lineformu1.c*lineformu2.a - lineformu2.c*lineformu1.a)
					/(lineformu1.a*lineformu2.b - lineformu2.a*lineformu1.b));
	}
	if((i*j==0)||(m*n==0))
		return 0;
	return 1;
}
int cn_2SectionsCross(dbPOINT A, dbPOINT B, dbPOINT C, dbPOINT D, dbPOINT* P)
{//to see if the 2 line segments, AB and CD, cross. If does, the returned 
// value is 1, the returned value is -1 if not.
	LINEFORMULATION line1, line2;
	StdLineForm(A, B, &line1);
	StdLineForm(C, D, &line2);
	int i = cn_2LinesCrossPoint(&line1, &line2, P);
	if(i==-1)
		return -1;//parallel, P meaningless
	if((cn_LineSegmentHoldin(*P, A, B)==1)&&(cn_LineSegmentHoldin(*P, C, D)==1))
		return 1;
	return -1;
}
double cn_RefractAngle(POINT A, POINT B, POINT C, POINT D)
{//To calculate the refraction angle line AB vs line CD, where
//AB is the master and CD is the slave. The result is between 0 - 2*π.
	double cn_pi = acos(-1.0);
	double masterangle = cn_LineAngle(A, B);
	double slaveangle = cn_LineAngle(C, D);
	if(slaveangle>cn_pi)
		slaveangle -= cn_pi;//limit the slave between 0 ~ π
	double delta = fabs(masterangle - slaveangle);
	double theta = slaveangle - delta;
	theta = theta>=0 ? theta : (2*cn_pi + theta);
	return theta; 
}
double cn_RefractAngle(dbPOINT A, dbPOINT B, dbPOINT C, dbPOINT D)
{//To calculate the refraction angle line AB vs line CD, where
//AB is the master and CD is the slave. The result is between 0 ~ 2*π.
	double cn_pi = acos(-1.0);
	double masterangle = cn_LineAngle(A, B);
	double slaveangle = cn_LineAngle(C, D);
	if(slaveangle>cn_pi)
		slaveangle = cn_AngleTrimPI(slaveangle);//limit the slave between 0 ~ π
	double delta = masterangle - slaveangle;
	double theta = slaveangle - delta;
	theta = cn_AngleTrim2PI(theta);//theta>=0 ? theta : (2*cn_pi + theta);
	return theta; 
}
double cn_PointLineDist(POINT P, POINT A, POINT B)
{//calculate the distance of point P and line AB
	double distance;
	if((A.x==B.x) && (A.y==B.y))//A,B为同一点,返回两点距离
		return sqrt((P.y-A.y)*(P.y-A.y) + (P.x-A.x)*(P.x-A.x));
	Formulation lineformu;
	StdLineForm(A, B, &lineformu);
	distance = (lineformu.a*P.x + lineformu.b*P.y + lineformu.c)/
				sqrt(lineformu.a*lineformu.a + lineformu.b*lineformu.b);
	return distance;
}

double cn_PointLineDist(dbPOINT Point, LINEFORMULATION *Line)
{
	return (Line->a * Point.x + Line->b * Point.y + Line->c)/
			sqrt(Line->a*Line->a + Line->b*Line->b);
}

double cn_PointLineDist(dbPOINT P, dbPOINT A, dbPOINT B)
{//calculate the distance of point P and line AB
	double distance;
	if((A.x==B.x) && (A.y==B.y))//A,B为同一点,返回两点距离
		return sqrt((P.y-A.y)*(P.y-A.y) + (P.x-A.x)*(P.x-A.x));
	Formulation lineformu;
	StdLineForm(A, B, &lineformu);
	distance = (lineformu.a*P.x + lineformu.b*P.y + lineformu.c)/
				sqrt(lineformu.a*lineformu.a + lineformu.b*lineformu.b);
	return distance;
}
double cn_2PointsDist(POINT A, POINT B)
{
	return sqrt((B.y-A.y)*(B.y-A.y) + (B.x-A.x)*(B.x-A.x));
}
double cn_2PointsDist(dbPOINT A, dbPOINT B)
{
	return sqrt((B.y-A.y)*(B.y-A.y) + (B.x-A.x)*(B.x-A.x));
}
double distRobot2Pt(RobotInford robot,dbPOINT point)//车到点的距离
{
	return sqrt((robot.x-point.x)*(robot.x-point.x) + (robot.y-point.y)*(robot.y-point.y));
}
double cn_AngleTrim2PI(double theta)
{////trim theta to between 0 and 2pi
	double cn_pi = acos(-1);
	if(theta<-100000000000 || theta >1000000)
		theta = theta;
	while(theta>=2*cn_pi)
		theta -= 2*pi;
	while(theta<0)
		theta += 2*pi;
	return theta;
}

double cn_AngleTrimPI(double theta)
{//trim theta to the angle between 0 and pi
	double cn_pi = acos(-1);
	theta = cn_AngleTrim2PI(theta);
	if(theta>cn_pi)
		theta -= cn_pi;
	return theta;
}

int cn_ArcSegmentCross( CIRCLEFORMULATION *pCircleFormulation, double sTheta, 
	double dTheta, dbPOINT A, dbPOINT B, dbPOINT *pResultA, dbPOINT *pResultB)
{
	dbPOINT p1, p2;
	if(cn_LineCircleCross(A, B, pCircleFormulation, &p1, &p2)==-1)
		return 0;//no cross point
	return 0;
}

int cn_LineSegmentHoldin(dbPOINT P, dbPOINT A, dbPOINT B)
{//to see if P is in the segment AB, return 1 if is, otherwise
//return 0;
	double d1, d2, d3;
	if(&P==NULL || &A==NULL || &B==NULL)
		return 0;
	d1 = cn_2PointsDist(P, A);
	d2 = cn_2PointsDist(P, B);
	d3 = cn_2PointsDist(A, B);
	if(fabs((d1+d2)-cn_2PointsDist(A, B))<0.000000001)
		return 1;
	return -1;
}

void CoordinateTransform(dbROBOTPOSTURE *pNewCoordiante, dbPOINT Target, dbPOINT *Result)
{
	dbPOINT newpoint;
	newpoint.x = Target.x - pNewCoordiante->x;//平移
	newpoint.y = Target.y - pNewCoordiante->y;
	//旋转
	Result->x = newpoint.x*cos(pNewCoordiante->theta) + newpoint.y*sin(pNewCoordiante->theta);
	Result->y = -newpoint.x*sin(pNewCoordiante->theta) + newpoint.y*cos(pNewCoordiante->theta);

}
int cn_SegmentBreak(SEGMENT *pSegment, dbPOINT SrcPoint, dbPOINT ObtPoint, double Radius,  SEGMENT *pSegment1, SEGMENT *pSegment2)
{
	dbPOINT c, d, temp;
	int count=1;
	double angle = cn_LineAngle(pSegment->A, pSegment->B);
	double dist1, dist2;
	dist1 = cn_PointLineDist(SrcPoint, pSegment->A, pSegment->B);
	dist2 = cn_PointLineDist(ObtPoint, pSegment->A, pSegment->B);
	if(dist2>=dist1)
	{
		*pSegment1 = *pSegment;
		return count;
	}
	temp.x = ObtPoint.x + Radius*cos(angle);
	temp.y = ObtPoint.y + Radius*sin(angle);
	cn_2LinesCrossPoint(pSegment->A, pSegment->B, SrcPoint, temp, &d);
	temp.x = ObtPoint.x + Radius*cos(angle + pi);
	temp.y = ObtPoint.y + Radius*sin(angle + pi);
	cn_2LinesCrossPoint(pSegment->A, pSegment->B, SrcPoint, temp, &c);
	if(cn_2PointsDist(SrcPoint, temp)>cn_2PointsDist(SrcPoint, c))
	{
		*pSegment1 = *pSegment;
		return count;
	}
	if(cn_LineSegmentHoldin(pSegment->A, c, d)==1 && 
		cn_LineSegmentHoldin(pSegment->B, c, d)==1)
		return count - 1;
	if(cn_LineSegmentHoldin(c, pSegment->A, pSegment->B)==-1 && 
		cn_LineSegmentHoldin(d, pSegment->A, pSegment->B)==-1)
	{
		*pSegment1 = *pSegment;
		return count;
	}
	if(cn_LineSegmentHoldin(c, pSegment->A, pSegment->B) == -1)
	{
		pSegment1->A = d;
		pSegment1->B = pSegment->B;
		return count;
	}
	if(cn_LineSegmentHoldin(d, pSegment->A, pSegment->B) == -1)
	{
		pSegment1->A = pSegment->A;
		pSegment1->B = c;
		return count;
	}
	pSegment1->A = pSegment->A;
	pSegment1->B = c;
	pSegment2->A = d;
	pSegment2->B = pSegment->B;
	return count+1;
}

int cn_SegmentBreak(SEGMENT *pSegment, int Number, dbPOINT SrcPoint, dbPOINT ObtPoint, double Radius)
{
	int i, j;
	SEGMENT s1, s2;
	for(i=0; i<Number; i++)
	{
		j = cn_SegmentBreak(&pSegment[i], SrcPoint, ObtPoint, Radius, &s1, &s2); 
		if(j == 1)
			pSegment[i] = s1;
		else if(j == 0)
		{
			for(j=i; j<Number; j++)//merge
				pSegment[j] = pSegment[j+1];
			Number--;
			i--;
		}
		else
		{
			for(j=Number; j>i; j--)//j==2, break
				pSegment[j] = pSegment[j-1];
			pSegment[i] = s1;
			pSegment[i+1] = s2;
			Number++;
			i++;
			break;//?
		}
	}
	return Number;
}

/*************** end *****************/
/************* zhou yunlong ***************/
int MiddleVerticalLine(dbPOINT point1,dbPOINT point2,Formulation *Formu)
{
	double x,y,k;
    x=(point1.x+point2.x)/2;
	y=(point1.y+point2.y)/2;
	if (point1.x==point2.x)
	{
		Formu->a=0;
		Formu->b=1;
		Formu->c=-y;
	}
	else
		if (point1.y==point2.y)
		{
			Formu->a=1;
			Formu->b=0;
			Formu->c=-x;
		}
		else
		{
			k=tan(atan((point2.y-point1.y)/(point2.x-point1.x))+pi/2);
			Formu->c=y-k*x;
			Formu->a=k;
			Formu->b=-1;
		}
	return(1);
}

int PointToPointDirectionAngle(dbPOINT Point1,dbPOINT Point2,double *pAngle)
//本函数用于求解从点point1到点point2的方向角，成功返回1，否则，返回0
//求得（0—2pi)之间的弧度方向角存于Angle, 
{
	double x,y;
	x=Point2.x-Point1.x;
	y=Point2.y-Point1.y;
	if (x==0 && y==0)
		return 0;
	*pAngle=atan2(y,x);
	if (*pAngle<0)
		*pAngle+=2*pi;
	return 1;
}
/*************** end ********************/