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

前言

  • 参考 *——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 C++】A*寻路算法
    【UE4 C++】A*寻路算法

  • 在 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; //除了第一个点,每个点都指向前一个点,可以通过它来绘制路径
    }
    

其他

上一篇:浙大数据结构07-图6 旅游规划


下一篇:Traveling Salesman among Aerial Cities(旅行商问题)