【UE4 C++】A*寻路算法

2021-09-08 14:55:28

前言

参考 *——A*搜索算法
虽然可以用BFS，DFS进行搜索，但是对于复杂地形来说，搜索成本会比较高。
其他寻路算法还有：Dijkstra算法、 JPS/JPS寻路算法，有兴趣可以再搜相关资料
实现版本 4.26
原创地址 cnblogs

A*寻路算法核心

A*算法的估算函数为：F(n) = G(n) + H(n)
- g(n) 表示从起点到 n点的实际距离（实践中使用的起点，其实是基于上一个点的g值再进行叠加）
- h(n) 表示 n 点到目标顶点的估算距离
- 距离一般用曼哈顿距离，支持四个方向，曼哈顿距离为起始点 | x1 - x2 | + | y1 + y2 | ；支持八个方向，则再加入对角线进行计算

整体算法

起点放到待遍历集合
while(目标没找到)
{	
	从待遍历集合取出Fn最小的点，作为当前计算点
	将当前计算点放入到已遍历集合中
	当前计算点上下左右的4个点进行遍历（如果支持斜方向，就是8个点）
		if (相邻点是墙 || 相邻点在已遍历集合中)
			进入下一个相邻点判断
		if (相邻点在待遍历集合)
			比较Gn，值比较小就更新 Fn、Gn、Hn 和其父节点
		else
			相邻点放到待遍历集合
} 
绘制路径

UE4 实现

效果

在 UE4 中，本次实现用以下结构体来表示点，并且在构造的时候计算 FGH 值

// 原文地址 https://www.cnblogs.com/shiroe/p/15516246.html
struct FPathPoint {
public:
	float cost_from_origin; // 到起点的曼哈顿距离，亦走了几步 Gn
	float cost_to_target;	// 到终点的曼哈顿距离，亦还要几步 Hn
	float total_cost;		// 曼哈顿距离之和，亦总的步数  Fn = Gn + Hn

	FVector pos;
	TSharedPtr<FPathPoint> pre_point;

		/*
	* 说明：起点的构造函数
	* @start_pos：起点位置
	* @target_pos：终点位置
	*/
	FPathPoint(FVector start_pos,FVector target_pos) {
		pos = start_pos;
		pre_point = nullptr;
		cost_from_origin = 0;
		cost_to_target = (FMath::Abs(target_pos.X - pos.X) + FMath::Abs(target_pos.Y - pos.Y)) / 100.0f;
		total_cost = cost_from_origin + cost_to_target;
	}

	/*
	* 说明：构造函数，计算曼哈顿距离 Fn = Gn + Hn
	* @_pos: 当前点的位置
	* @_pre_point: 前一点
	* @start_pos：起点位置
	* @target_pos：终点位置
	*/
	FPathPoint(FVector _pos, TSharedPtr<FPathPoint> _pre_point, FVector start_pos,FVector target_pos) {
		check(_pre_point != nullptr);
		pos = _pos;
		pre_point = _pre_point;

		cost_from_origin =  pre_point->cost_from_origin;
		cost_from_origin += (FMath::Abs(pre_point->pos.X - pos.X) + FMath::Abs(pre_point->pos.Y - pos.Y)) / 100.0f;

		cost_to_target = (FMath::Abs(target_pos.X - pos.X) + FMath::Abs(target_pos.Y - pos.Y)) / 100.0f;
		total_cost = cost_from_origin + cost_to_target;
	}

	//用于查找,对于智能指针无效，需在全局写一个
	bool operator ==(const FPathPoint& a) const{ return this->pos.Equals(a.pos, 1.0f); }
	//用于比较,对于智能指针无效，需在全局写一个
	bool operator <(const FPathPoint& a) { return this->cost_from_origin < a.cost_from_origin; }
}; 

//用于排序比较
bool operator <(const TSharedPtr<FPathPoint>& a,const TSharedPtr<FPathPoint>& b) 
{ 
	return a->cost_from_origin < b->cost_from_origin; 
}

//用于数组里的查找
bool operator ==(const TSharedPtr<FPathPoint>& a,const TSharedPtr<FPathPoint>& b) 
{ 
	return a->pos.Equals(b->pos, 1.0f); 
}

基于之前的迷宫实现，添加以下数据和方法，用于初始化地形，和随机生成起始点

头文件

// 原文地址 https://www.cnblogs.com/shiroe/p/15516246.html
void FinishedCreation();

UPROPERTY(EditAnywhere)
	UArrowComponent* startPoint; //随机生成的起点
UPROPERTY(EditAnywhere)
	UArrowComponent* targetPoint; //随机生成的终点

TArray<TSharedPtr<FPathPoint>> open_set; //待遍历的节点
TArray<TSharedPtr<FPathPoint>> close_set;// 已遍历的节点
TSharedPtr<FPathPoint> currentpoint;

void AStar_FindPath(); //寻找路径
void AStar_DrawPath(); //绘制路径

cpp

void AMazeCreator::FinishedCreation()
{
	if (timerHandle.IsValid()) {
		GetWorld()->GetTimerManager().ClearTimer(timerHandle);
	}
	startPoint = NewObject<UArrowComponent>(this);
	startPoint->SetupAttachment(rootComp);
	startPoint->RegisterComponent();
	startPoint->SetRelativeLocation(roadrMeshs[height+1]->GetRelativeLocation());
	startPoint->SetRelativeRotation(FRotator(90, 0, 0));
	startPoint->ArrowSize = 4;
	startPoint->SetHiddenInGame(false);

	targetPoint = NewObject<UArrowComponent>(this);
	targetPoint->SetupAttachment(rootComp);
	targetPoint->RegisterComponent();
	targetPoint->SetRelativeRotation(FRotator(90, 0, 0));
	targetPoint->ArrowSize = 4;
	targetPoint->SetHiddenInGame(false);
			
	while (true)
	{
		int32 x = UKismetMathLibrary::RandomIntegerInRange(width / 2, width-1);
		int32 y = UKismetMathLibrary::RandomIntegerInRange(height / 2, height-1);
		if (roadArr[x][y]==1)
		{
			targetPoint->SetRelativeLocation(roadrMeshs[height * x + y]->GetRelativeLocation());
			break;
		}
	}

	//第一个点放入到待遍历列表
	open_set.HeapPush(
		MakeShareable (
			new FPathPoint( 
			startPoint->GetComponentLocation(), 
			targetPoint->GetComponentLocation())
		));
	
	GetWorld()->GetTimerManager().SetTimer(timerHandle,this, &AMazeCreator::AStar_FindPath, 0.04f, true,1);
}

主要寻路算法

// 原文地址 https://www.cnblogs.com/shiroe/p/15516246.html
void AMazeCreator::AStar_FindPath()
{
	if (open_set.Num() > 0) { //待遍历列表不为空

		open_set.HeapPop(currentpoint); //待遍历堆取出 Fn最小的点
		close_set.Add(currentpoint);  //将取出的点放入到已经遍历的列表中

		//若到达目标点，停止继续查询
		if (currentpoint->pos.Equals(targetPoint->GetComponentLocation(), 1.0f)) {
			GetWorld()->GetTimerManager().ClearTimer(timerHandle);
			GetWorld()->GetTimerManager().SetTimer(timerHandle,this, &AMazeCreator::AStar_DrawPath, 0.04f, true,1);
			UE_LOG(LogTemp, Warning, TEXT("find target "));
			return;
		}

		TArray <TEnumAsByte<EObjectTypeQuery>>  ObjectTypes;
		ObjectTypes.Add(EObjectTypeQuery::ObjectTypeQuery2);

		TArray <AActor*> ActorsToIgnore;
		ActorsToIgnore.Empty();
		FHitResult OutHit;
		bool bIgnoreSelf = false;

		// 四个方向检测，此处用射线
		TArray<float> dirX = { -1, 0, 1, 0 };
		TArray<float> dirY = { 0, -1, 0, 1 };
		for (int i = 0; i < 4; i++)
		{
			if (!UKismetSystemLibrary::LineTraceSingleForObjects(
				GetWorld(),
				currentpoint->pos + FVector(0, 0, 50),
				currentpoint->pos + FVector(dirX[i] * 100, dirY[i] * 100, - 10),
				ObjectTypes,
				false,
				ActorsToIgnore,
				EDrawDebugTrace::ForDuration,
				OutHit,
				bIgnoreSelf
			)) {
				continue;
			}
			
			//判断找到的是否是道路
			UStaticMeshComponent* smComp = Cast<UStaticMeshComponent>(OutHit.GetComponent());
			if (smComp && OutHit.Location.Z < -50.f+0.001f ) {  //此处用z轴值筛出是道路
				TSharedPtr<FPathPoint> point = MakeShareable(new FPathPoint(
					smComp->GetComponentLocation(),
					currentpoint,
					startPoint->GetComponentLocation(),
					targetPoint->GetComponentLocation())
				);

				if(close_set.Contains(point)) //判断点是否已经遍历过
					continue;
				
				int idx = open_set.Find(point); //判断点是否在待遍历列表中，如果是就更新消耗比较小的值；如果不在，则直接添加
				if (idx!=INDEX_NONE)
				{
					UE_LOG(LogTemp, Warning, TEXT("Find repeat point!"));					
					if (point.Get() < open_set[idx].Get()) {
						open_set[idx]->cost_from_origin = point->cost_from_origin;
						open_set[idx]->total_cost = point->total_cost;
						open_set[idx]->pre_point = point->pre_point;
					}

				}else{
					open_set.HeapPush(point);
				}
			}
		}
	}else
		UE_LOG(LogTemp, Warning, TEXT("open_set is null "));
}

绘制路径

// 原文地址 https://www.cnblogs.com/shiroe/p/15516246.html
void AMazeCreator::AStar_DrawPath()
{
	if (!currentpoint.IsValid())
		return;

	if (!currentpoint->pre_point.IsValid()) {
		GetWorld()->GetTimerManager().ClearTimer(timerHandle);
		return;
	}

	UKismetSystemLibrary::DrawDebugPlane(GetWorld(),FPlane( FVector(0,0,25), FVector::UpVector),
					currentpoint->pos, 50, FLinearColor::Green,5);
	currentpoint = currentpoint->pre_point; //除了第一个点，每个点都指向前一个点，可以通过它来绘制路径
}

其他

A* Pathfinding for Beginners
保姆级蓝图解说 —— UE4-A星寻路制作

码农公寓

前言

A*寻路算法核心

UE4 实现

其他

相关文章