obstacleavoidance.cpp

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

// ObstacleAvoidance.cpp: implementation of the CObstacleAvoidance class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "math.h"
#include "ObstacleAvoidance.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
/*
CObstacleAvoidance::CObstacleAvoidance(Environment *envPointer, UserDefStruct *UserDataPointer)
{
	UserData = UserDataPointer;
	env = envPointer;
	ObstacleAvoidanceData.NumberOfObstacle = 0;
}

CObstacleAvoidance::~CObstacleAvoidance()
{
}

//Currently In Development
void CObstacleAvoidance::ExecuteObstacleAvoidance(Vector3D StartingPoint, Vector3D EndingPoint, Vector3D &AvoidancePoint, BOOL &NeedToAvoid)
{
	ObstacleAvoidanceData.StartingPoint = StartingPoint;
	ObstacleAvoidanceData.EndingPoint = EndingPoint;
	DetermineObstacle(ObstacleAvoidanceData);
	if(ObstacleAvoidanceData.NumberOfObstacle == 0)
	{
		NeedToAvoid = FALSE;
	}
	else
	{
		CalculateAvoidancePoint(ObstacleAvoidanceData);
		AvoidancePoint = SelectAvoidancePoint(ObstacleAvoidanceData);
		NeedToAvoid = TRUE;
	}
	return;
}

void CObstacleAvoidance::DetermineObstacle(ObstacleAvoidanceStruct &ObstacleAvoidanceData)
{
	double StartingToEndingPointDistance, StartingToEndingPointAngleDegree, StartingToEndingPointAngleRadian;
	Vector3D TempObjectPosition;
	double TempObjectAngleDegree;
	double TempStartingPointToObjectAngle;
	double TempStartingPointToObjectDistance;
	ObstacleAvoidanceStruct TempObstacleAvoidanceData;
	
	StartingToEndingPointDistance = CalculatePointToPointDistance(ObstacleAvoidanceData.StartingPoint ,ObstacleAvoidanceData.EndingPoint);
	StartingToEndingPointAngleDegree = CalculatePointToPointAngleDegree(ObstacleAvoidanceData.StartingPoint ,ObstacleAvoidanceData.EndingPoint);
	StartingToEndingPointAngleRadian = CalculatePointToPointAngleRadian(ObstacleAvoidanceData.StartingPoint ,ObstacleAvoidanceData.EndingPoint);

	//Determine Obstacle
	TempObstacleAvoidanceData.NumberOfObstacle = 0;
	for(int i=0;i<5;i++)
	{
		//Check In Home Robots
		TempObjectPosition =UserData->HomeRobotData[i].Position;
		if( (ObstacleAvoidanceData.StartingPoint.x != TempObjectPosition.x) && (ObstacleAvoidanceData.StartingPoint.y != TempObjectPosition.y) ) //Do not check itself
		{
			TempObjectAngleDegree = UserData->HomeRobotData[i].Angle.Degree;
			TempStartingPointToObjectDistance = CalculatePointToPointDistance(ObstacleAvoidanceData.StartingPoint, TempObjectPosition);
			TempStartingPointToObjectAngle = CalculatePointToPointAngleDegree(ObstacleAvoidanceData.StartingPoint, TempObjectPosition);
			if( CheckObstacleWithinPath(ObstacleAvoidanceData.StartingPoint, ObstacleAvoidanceData.EndingPoint, TempObjectPosition) )
			{
				TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].ObstacleType = 0;
				TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].Position = TempObjectPosition;
				TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].Angle.Degree = TempObjectAngleDegree;
				TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].DistanceFromStartingPoint = TempStartingPointToObjectDistance;
				TempObstacleAvoidanceData.NumberOfObstacle++;
			}
		}

		//Check In Opponent Robot
		TempObjectPosition = UserData->OpponentRobotData[i].Position;
		TempStartingPointToObjectDistance = CalculatePointToPointDistance(ObstacleAvoidanceData.StartingPoint, TempObjectPosition);
		TempStartingPointToObjectAngle = CalculatePointToPointAngleDegree(ObstacleAvoidanceData.StartingPoint, TempObjectPosition);
		if( CheckObstacleWithinPath(ObstacleAvoidanceData.StartingPoint, ObstacleAvoidanceData.EndingPoint, TempObjectPosition) )
		{
			TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].ObstacleType = 1;
			TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].Position = TempObjectPosition;
			TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].DistanceFromStartingPoint = TempStartingPointToObjectDistance;
			TempObstacleAvoidanceData.NumberOfObstacle++;
		}
	}
/*
	if( (TempObstacleAvoidanceData.StartingPoint.x != globaldata.StrategySimData.BallPosP.x) && (TempObstacleAvoidanceData.StartingPoint.y != globaldata.StrategySimData.BallPosP.y) )
	{
		TempObjectPosition = UserData->BallData.CurrentBall.Position;
		TempStartingPointToObjectDistance = CalculatePointToPointDistance(ObstacleAvoidanceData.StartingPoint, TempObjectPosition);
		TempStartingPointToObjectAngle = CalculatePointToPointAngleDegree(ObstacleAvoidanceData.StartingPoint, TempObjectPosition);
		if(fabs(StartingToEndingPointAngleDegree - TempStartingPointToObjectAngle) < OffsetAngle)
		{
			TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].ObstacleType = 2;
			TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].Position = TempObjectPosition;
			TempObstacleAvoidanceData.ObstacleData[TempObstacleAvoidanceData.NumberOfObstacle].DistanceFromStartingPoint = TempStartingPointToObjectDistance;
			TempObstacleAvoidanceData.NumberOfObstacle++;
		}
	}*/
	//End Of Determine Obstacle
/*
	//Sorting Out Obstacle
	SortingObstacleInDistance(TempObstacleAvoidanceData, ObstacleAvoidanceData);
	//End Of Sorting Out Obstacle;
	TempObstacleAvoidanceData.StartingPoint = ObstacleAvoidanceData.StartingPoint;
	TempObstacleAvoidanceData.EndingPoint = ObstacleAvoidanceData.EndingPoint;
	ObstacleAvoidanceData = TempObstacleAvoidanceData;
	return;
}

BOOL CObstacleAvoidance::CheckObstacleWithinPath(Vector3D StartingPoint, Vector3D EndingPoint, Vector3D ObstaclePoint)
{
	double ObstacleTolerance = 45;
	double StartingPointToEndingPointAngleDegree;
	double EndingPointToStartingPointAngleDegree;
	double StartingPointToObstacleAngleDegree;
	double EndingPointToObstacleAngleDegree;
	double AngleFactor1, AngleFactor2;

	StartingPointToEndingPointAngleDegree = CalculatePointToPointAngleDegree(StartingPoint, EndingPoint);
	EndingPointToStartingPointAngleDegree = CalculatePointToPointAngleDegree(EndingPoint, StartingPoint);
	StartingPointToObstacleAngleDegree = CalculatePointToPointAngleDegree(StartingPoint, ObstaclePoint);
	EndingPointToObstacleAngleDegree = CalculatePointToPointAngleDegree(EndingPoint, ObstaclePoint);

	AngleFactor1 = fabs(StartingPointToEndingPointAngleDegree - StartingPointToObstacleAngleDegree);

	if(AngleFactor1 > 180)
	{
		AngleFactor1 = 360 - AngleFactor1;
	}

	AngleFactor2 = fabs(EndingPointToStartingPointAngleDegree - EndingPointToObstacleAngleDegree);

	if(AngleFactor2 > 180)
	{
		AngleFactor2 = 360 - AngleFactor2;
	}

	if( (AngleFactor1 < ObstacleTolerance) && (AngleFactor2 < ObstacleTolerance) )
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}

}

void CObstacleAvoidance::SortingObstacleInDistance(ObstacleAvoidanceStruct &TempObstacleAvoidanceData, ObstacleAvoidanceStruct &ObstacleAvoidanceData)
{
	double TempDistance1, TempDistance2;
	int Counter, MaxObject;
	MaxObject = TempObstacleAvoidanceData.NumberOfObstacle;
	Counter = 0;
	while(Counter != MaxObject)
	{
		for(int i=0;i<MaxObject-1;i++)
		{
			TempDistance1 = TempObstacleAvoidanceData.ObstacleData[i].DistanceFromStartingPoint;
			TempDistance2 = TempObstacleAvoidanceData.ObstacleData[i+1].DistanceFromStartingPoint;
			if(TempDistance1 > TempDistance2)
			{
				ObstacleAvoidanceData.ObstacleData[i] = TempObstacleAvoidanceData.ObstacleData[i+1];
				ObstacleAvoidanceData.ObstacleData[i+1] = TempObstacleAvoidanceData.ObstacleData[i];

				TempObstacleAvoidanceData.ObstacleData[i] = ObstacleAvoidanceData.ObstacleData[i];
				TempObstacleAvoidanceData.ObstacleData[i+1] = ObstacleAvoidanceData.ObstacleData[i+1];
			}
		}
		Counter++;
	}
	return;
}

void CObstacleAvoidance::ObstacleGrouping(ObstacleAvoidanceStruct &ObstacleAvoidanceData)
{
	int MaxObject, TempMaxObject;
	double ObstacleGroupingFactor = 7.5;
	double DistanceBetweenObstacle;
	BYTE Group = 0;
	MaxObject = TempMaxObject = ObstacleAvoidanceData.NumberOfObstacle;

	//Initialize Obstacle Group
	for(int i=0;i<MaxObject-1;i++)
	{
		ObstacleAvoidanceData.ObstacleData[i].Group = 255; //255 Means No Group
	}

	//Differentiate The Group
	for(i=0;i<MaxObject-1;i++)
	{
		DistanceBetweenObstacle = CalculatePointToPointDistance(ObstacleAvoidanceData.ObstacleData[i].Position, ObstacleAvoidanceData.ObstacleData[i+1].Position);
		if(DistanceBetweenObstacle < ObstacleGroupingFactor)
		{
			if(i == 0) //Means First Object
			{
				ObstacleAvoidanceData.ObstacleData[i].Group = Group;
				Group++;
			}
			else
			{
				if(ObstacleAvoidanceData.ObstacleData[i-1].Group != 255)
				{
					ObstacleAvoidanceData.ObstacleData[i].Group = ObstacleAvoidanceData.ObstacleData[i-1].Group;
				}
				else
				{
					ObstacleAvoidanceData.ObstacleData[i].Group = Group;
					Group++;
				}
			}
		}
	}
	return;
}

void CObstacleAvoidance::CalculateAvoidancePoint(ObstacleAvoidanceStruct &ObstacleAvoidanceData)
{
	int MaxObject;
	double AvoidanceDistance = 7.5;
	double AvoidancePointFactor = (4.0 / 4.0);
	MaxObject = ObstacleAvoidanceData.NumberOfObstacle;

	for(int i=0;i<MaxObject;i++)
	{
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0].x = ObstacleAvoidanceData.ObstacleData[i].Position.x;
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0].y = ObstacleAvoidanceData.ObstacleData[i].Position.y + AvoidanceDistance;

		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1].x = ObstacleAvoidanceData.ObstacleData[i].Position.x + (AvoidanceDistance * AvoidancePointFactor);
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1].y = ObstacleAvoidanceData.ObstacleData[i].Position.y + (AvoidanceDistance * AvoidancePointFactor);

		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2].x = ObstacleAvoidanceData.ObstacleData[i].Position.x + AvoidanceDistance;
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2].y = ObstacleAvoidanceData.ObstacleData[i].Position.y;
		
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].x = ObstacleAvoidanceData.ObstacleData[i].Position.x + (AvoidanceDistance * AvoidancePointFactor);
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[3].y = ObstacleAvoidanceData.ObstacleData[i].Position.y - (AvoidanceDistance * AvoidancePointFactor);

		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4].x = ObstacleAvoidanceData.ObstacleData[i].Position.x;
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[4].y = ObstacleAvoidanceData.ObstacleData[i].Position.y - AvoidanceDistance;

		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].x = ObstacleAvoidanceData.ObstacleData[i].Position.x - (AvoidanceDistance * AvoidancePointFactor);
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[5].y = ObstacleAvoidanceData.ObstacleData[i].Position.y - (AvoidanceDistance * AvoidancePointFactor);
		
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6].x = ObstacleAvoidanceData.ObstacleData[i].Position.x - AvoidanceDistance;
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[6].y = ObstacleAvoidanceData.ObstacleData[i].Position.y;

		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].x = ObstacleAvoidanceData.ObstacleData[i].Position.x - (AvoidanceDistance * AvoidancePointFactor);
		ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[7].y = ObstacleAvoidanceData.ObstacleData[i].Position.y + (AvoidanceDistance * AvoidancePointFactor);
	}
	return;
}

void CObstacleAvoidance::SetGroupObstacleAvoidancePoint(ObstacleAvoidanceStruct &ObstacleAvoidanceData)
{
	Vector3D TempAvoidancePoint;
	int MaxObject;
	int i;
	BYTE PreviousGroup = 255;
	MaxObject = ObstacleAvoidanceData.NumberOfObstacle;

	//Setting New Avoidance Point If Group Is Avaliable
	
	//For Point 0
	for(i=0;i<MaxObject;i++)
	{
		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 0, We Take The Highest Y From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0].y > TempAvoidancePoint.y)
					{
						TempAvoidancePoint.y = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0].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[0].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[0];
			}
		}
	}

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

					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1].y > TempAvoidancePoint.y)
					{
						TempAvoidancePoint.y = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1].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[1].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[1];
			}
		}
	}

	PreviousGroup = 255;
	//For Point 2
	for(i=0;i<MaxObject;i++)
	{
		if(ObstacleAvoidanceData.ObstacleData[i].Group != 255)
		{
			//For Point 2, We Take The Highest X From The Group
			if(PreviousGroup != 255)
			{
				if(ObstacleAvoidanceData.ObstacleData[i].Group == PreviousGroup)
				{
					if(ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2].x > TempAvoidancePoint.x)
					{
						TempAvoidancePoint.x = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2].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[2].x = TempAvoidancePoint.x;
					ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2].y = TempAvoidancePoint.y;
					TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2];
					PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				}
			}
			else
			{
				//Means That A Group Is Available
				PreviousGroup = ObstacleAvoidanceData.ObstacleData[i].Group;
				TempAvoidancePoint = ObstacleAvoidanceData.ObstacleData[i].AvoidancePoint[2];
			}
		}
	}

	PreviousGroup = 255;
	//For Point 3
	for(i=0;i<MaxObject;i++)
	{