obstacleavoidance.cpp

FIRA 5V5比赛中一个机器人源代码 本科毕业设计做的

		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 3, We Take The Highest X And Lowest Y From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].x > TempAvoidancePoint.x)
					{
						TempAvoidancePoint.x = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].x;
					}

					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].y < TempAvoidancePoint.y)
					{
						TempAvoidancePoint.y = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].y;
					}
				}
				else
				{
					//Means That A New Group Is Available
					//When Changing Group We Save The First Group Data To Slot 0
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3];
			}
		}
	}

	PreviousGroup = 255;
	//For Point 4
	for(i=0;i<MaxObject;i++)
	{
		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 4, We Take The Lowest Y From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4].y < TempAvoidancePoint.y)
					{
						TempAvoidancePoint.y = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4].y;
					}
				}
				else
				{
					//Means That A New Group Is Available
					//When Changing Group We Save The First Group Data To Slot 0
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4];
			}
		}
	}

	PreviousGroup = 255;
	//For Point 5
	for(i=0;i<MaxObject;i++)
	{
		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 5, We Take The Lowest X And Lowest Y From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].x < TempAvoidancePoint.x)
					{
						TempAvoidancePoint.x = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].x;
					}

					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].y < TempAvoidancePoint.y)
					{
						TempAvoidancePoint.y = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].y;
					}
				}
				else
				{
					//Means That A New Group Is Available
					//When Changing Group We Save The First Group Data To Slot 0
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5];
			}
		}
	}

	PreviousGroup = 255;
	//For Point 6
	for(i=0;i<MaxObject;i++)
	{
		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 6, We Take The Lowest X From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6].x < TempAvoidancePoint.x)
					{
						TempAvoidancePoint.x = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6].x;
					}
				}
				else
				{
					//Means That A New Group Is Available
					//When Changing Group We Save The First Group Data To Slot 0
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6];
			}
		}
	}

	PreviousGroup = 255;
	//For Point 7
	for(i=0;i<MaxObject;i++)
	{
		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 7, We Take The Lowest X And Highest Y From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].x < TempAvoidancePoint.x)
					{
						TempAvoidancePoint.x = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].x;
					}

					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].y > TempAvoidancePoint.y)
					{
						TempAvoidancePoint.y = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].y;
					}
				}
				else
				{
					//Means That A New Group Is Available
					//When Changing Group We Save The First Group Data To Slot 0
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7];
			}
		}
	}
}

Vector3D CObstacleAvoidance::SelectAvoidancePoint(ObstacleAvoidanceStruct &ObstacleAvoidanceData)
{
	double ObstacleToStartPointAngleDegree, ObstacleToEndPointAngleDegree;
	int MaxObject;
	BYTE PreviousGroup = 255;
	MaxObject = ObstacleAvoidanceData.NumberOfObstacle;

	SetGroupObstacleAvoidancePoint(ObstacleAvoidanceData);

	ObstacleToStartPointAngleDegree = CalculatePointToPointAngleDegree(ObstacleAvoidanceData.ObstacleData[0].Position,ObstacleAvoidanceData.StartingPoint);
	ObstacleToEndPointAngleDegree = CalculatePointToPointAngleDegree(ObstacleAvoidanceData.ObstacleData[0].Position,ObstacleAvoidanceData.EndingPoint);

	//For 0 To 90 Degree
	if(ObstacleToEndPointAngleDegree >= 0 && ObstacleToEndPointAngleDegree < 90)
	{
		if(ObstacleToStartPointAngleDegree >= 90 && ObstacleToStartPointAngleDegree < 180)
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[0];
		}
		else if(ObstacleToStartPointAngleDegree >= 180 && ObstacleToStartPointAngleDegree < 270)
		{
			if(ObstacleToStartPointAngleDegree >= 180 && ObstacleToStartPointAngleDegree < 225)
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[7];
			}
			else
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[3];
			}
		}
		else
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[2];
		}
	}

	//For 90 To 180 Degree
	if(ObstacleToEndPointAngleDegree >= 90 && ObstacleToEndPointAngleDegree < 180)
	{
		if(ObstacleToStartPointAngleDegree >= 0 && ObstacleToStartPointAngleDegree < 90)
/*		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[0];
		}
		else if(ObstacleToStartPointAngleDegree >= 180 && ObstacleToStartPointAngleDegree < 270)
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[6];
		}
		else
		{
			if(ObstacleToStartPointAngleDegree >= 270 && ObstacleToStartPointAngleDegree < 315)
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[5];
			}
			else
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[1];
			}
		}
	}

	//For 180 To 270 Degree
	if(ObstacleToEndPointAngleDegree >= 180 && ObstacleToEndPointAngleDegree < 270)
	{
		if(ObstacleToStartPointAngleDegree >= 0 && ObstacleToStartPointAngleDegree < 90)
		{
			if(ObstacleToStartPointAngleDegree >= 0 && ObstacleToStartPointAngleDegree < 45)
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[3];
			}
			else
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[7];
			}
		}
		else if(ObstacleToStartPointAngleDegree >= 90 && ObstacleToStartPointAngleDegree < 180)
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[6];
		}
		else
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[4];
		}
	}
	
	//For 270 To 360 Degree
	if(ObstacleToEndPointAngleDegree >= 270 && ObstacleToEndPointAngleDegree < 360)
	{
		if(ObstacleToStartPointAngleDegree >= 0 && ObstacleToStartPointAngleDegree < 90)
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[2];
		}
		else if(ObstacleToStartPointAngleDegree >= 90 && ObstacleToStartPointAngleDegree < 180)
		{
			if(ObstacleToStartPointAngleDegree >= 90 && ObstacleToStartPointAngleDegree < 135)
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[1];
			}
			else
			{
				return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[5];
			}
		}
		else
		{
			return ObstacleAvoidanceData.ObstacleData[0].AvoidancePoint[4];
		}
	}
}

BOOL CObstacleAvoidance::CheckPointWithinObstacle(ObstacleAvoidanceStruct &ObstacleAvoidanceData, Vector3D &AvoidancePoint, int ObstacleID)
{
	int MaxObject;
	double AvoidanceToleranceDistance = 7.5;
	double AvoidancePointToObstacleDistance;
	MaxObject = ObstacleAvoidanceData.NumberOfObstacle;

	for(int i=0;i<MaxObject;i++)
	{
		AvoidancePointToObstacleDistance = CalculatePointToPointDistance(AvoidancePoint, ObstacleAvoidanceData.ObstacleData[i].Position);

		if( (AvoidancePointToObstacleDistance < AvoidanceToleranceDistance) && (i != ObstacleID) )
		{
			return TRUE;
		}
	}

	return FALSE;
}

double CObstacleAvoidance::CalculatePointToPointDistance(Vector3D StartPoint, Vector3D EndPoint)
{
	double dy, dx, Distance;
	dy = EndPoint.y - StartPoint.y;
	dx = EndPoint.x - StartPoint.x;
	Distance = sqrt( (dy * dy) + (dx * dx) );
	return Distance;
}

double CObstacleAvoidance::CalculatePointToPointAngleDegree(Vector3D StartPoint, Vector3D EndPoint)
{
	/*
	For 0 - 360 Degree
	/////////////////
	/     90        /
	/      |        /
	/ 180 - - 0/360 /
	/      |        /
	/	  270       /
	/////////////////
	*/
/*
	double dy, dx, Angle;
	dy = EndPoint.y - StartPoint.y;
	dx = EndPoint.x - StartPoint.x;
	Angle = ( atan2(dy, dx) ) * ( 180.0 / PI );

	if(Angle < 0)
	{
		Angle = 360 + Angle;
	}

	return Angle;
}

double CObstacleAvoidance::CalculatePointToPointAngleRadian(Vector3D StartPoint, Vector3D EndPoint)
{
	/*
	For 0 - 360 Degree
	/////////////////
	/     90        /
	/      |        /
	/ 180 - - 0/360 /
	/      |        /
	/	  270       /
	/////////////////
	*/
/*
	double dy, dx, Angle;
	dy = EndPoint.y - StartPoint.y;
	dx = EndPoint.x - StartPoint.x;
	Angle = ( atan2(dy, dx) ) * ( 180.0 / PI );

	if(Angle < 0)
	{
		Angle = 360 + Angle;
	}

	Angle = Angle * (PI / 180.0);

	return Angle;
}*/