Big rework to A*; factored out path linking

AdventureMap.cpp

@@ -7,7 +7,6 @@
 #include "AdventureCharacter.h"
 #include "Kismet/GameplayStatics.h"
 #include "Algo/Reverse.h"
-#include <unordered_map>
 
 // Sets default values
 AAdventureMap::AAdventureMap()
@@ -32,21 +31,17 @@ void AAdventureMap::MakeGrid()
 	float HexWidth = sqrt(3) * TileSize;
 	int QOffset = 0;
 
-	for (int r = 1; r <= GridSize; r++) 
-	{
+	for (int r = 1; r <= GridSize; r++) {
 		float XOffset = 0.f;
 
-		if (r % 2 != 0)
-		{
-			if (r > 1)
-			{
+		if (r % 2 != 0)	{
+			if (r > 1) {
 				QOffset--;
 			}
 		}
 		else { XOffset = HexWidth / 2; }
 
-		for (int q = 1; q <= GridSize; q++)
-		{
+		for (int q = 1; q <= GridSize; q++)	{
 			NextHexAt.X = XOffset + (HexWidth * q);
 			NextHexAt.Y = TileSize * 1.5f * r;
 			NextHexAt.Z = 0.f;
@@ -61,8 +56,7 @@ void AAdventureMap::MakeGrid()
 		}
 	}
 
-	for (auto& tile : Grid)
-	{
+	for (auto& tile : Grid)	{
 		tile->Index = GridIndex(tile->Q, tile->R);
 	}
 
@@ -95,77 +89,69 @@ TArray<AHexTile*> AAdventureMap::Neighbors(AHexTile* OfHex)
 	TArray<int32> Indeces;
 	Indeces.Append(Arr, UE_ARRAY_COUNT(Arr));
 
-	Neighbors.Add(RandomHex());
+	Neighbors.Add(RandomHex()); // pathetic but necessary
 	for (auto& I : Indeces)	{ if (Grid.IsValidIndex(I) && OfHex->Distance(Grid[I]) == 1) { Neighbors.Add(Grid[I]); } }
 	return Neighbors;
 }
 
-TArray<AHexTile*> AAdventureMap::FindPathAStar(AHexTile* Start, AHexTile* Goal)
+TArray<AHexTile*> AAdventureMap::AStar(AHexTile* Start, AHexTile* Goal)
 {
-	TArray<AHexTile*> PQ; // Makeshift Priority Queue.	(High value = Low priority)
-	PQ.Add(Start);
+	TArray<AHexTile*> ToExamine;
+	TArray<AHexTile*> Processed;
+	ToExamine.Add(Start);
 
-	// The goal of this loop is to edit the Hex->CameFrom pointers, So as to chain Hexes from Goal to Start
-	while (!PQ.IsEmpty())
-	{
-		AHexTile* Current = PQ[0];
-		PQ.RemoveAt(0);
-		// UE_LOG(LogTemp, Warning, TEXT("Popping top priority Hex %d:%d"), Current->Q, Current->R); // debug
+	while (!ToExamine.IsEmpty())	{
+		AHexTile* Candidate = ToExamine[0];
 
-		if (Current == Goal) // early exit
-		{ 
-			// UE_LOG(LogTemp, Warning, TEXT("Goal found!!!")); // debug
-			break; 
+		// estimate closest known Hex to Goal
+		for (auto& t : ToExamine) {
+			int32 FCost = t->GCost + t->HCost;
+			if (t->FCost < Candidate->FCost || t->FCost == Candidate->FCost && t->HCost < Candidate->HCost) {	Candidate = t; }
 		}
 
-		// UE_LOG(LogTemp, Warning, TEXT("Expanding the frontier...")); // debug
-		for (AHexTile* Next : Neighbors(Current))
-		{
-			int32 NewCost = Current->CostSoFar + Next->MoveCost;			
-			if (NewCost < Next->CostSoFar || !PQ.Contains(Next))
-			{
-				Next->CostSoFar = NewCost;
-				Next->CameFrom = Current;
-				PQ.Remove(Next);
-				int32 NewPrio = NewCost + Next->Distance(Goal); // Higher value = Lower priority
+		Processed.Add(Candidate);
+		ToExamine.Remove(Candidate);
 
-				// no crash up to this point
+		// exit
+		if (Candidate == Goal) { break; }
 
-				// UE_LOG(LogTemp, Warning, TEXT("Hex %d:%d path info updated. Readjusting priorities..."), Next->Q, Next->R); // debug
-				if (!PQ.IsEmpty())
-				{
-					int32 OldPrio;
-					int32 OldIndex = PQ.Num() - 1; // default to lowest priority
+		// expand frontier & adjust path data
+		for (AHexTile* Neighbor : Neighbors(Candidate)) {
+			if (Processed.Contains(Neighbor)) { continue; }
 
-					for (auto& Hex : PQ)
-					{
-						OldPrio = Hex->CostSoFar + Hex->Distance(Goal);
-						if (NewPrio <= OldPrio) // looking for 1. Hex in "PQ" with a lower priority than that of "Next"
-						{
-							PQ.Find(Hex, OldIndex); // redefine OldIndex
-							break;
-						}
-					}
-					// UE_LOG(LogTemp, Warning, TEXT("Saving at PQ index %d"), OldIndex); // debug
-					PQ.Insert(Next, OldIndex);
-				}
-				else
-				{
-					PQ.Add(Next);
-					// UE_LOG(LogTemp, Warning, TEXT("Adding (PQ was empty)")); // debug
+			bool bInToExamine = ToExamine.Contains(Neighbor);
+			int32 NewGCost = Candidate->GCost + Neighbor->MoveCost;
+
+			if (NewGCost < Neighbor->GCost || !bInToExamine) {
+				Neighbor->GCost = NewGCost;
+				Neighbor->CameFrom = Candidate; // chain
+
+				if (!bInToExamine)	{
+					Neighbor->HCost = Neighbor->Distance(Goal);
+					ToExamine.Add(Neighbor);
 				}
 			}
 		}	
 	}
+
+	return LinkPath(Start, Goal);
+}
+
+TArray<AHexTile*> AAdventureMap::LinkPath(AHexTile* Start, AHexTile* Goal)
+{
 	TArray<AHexTile*> Path;
-	/*
-	AHexTile* Hex = Goal;
-	while (Hex != Start)
-	{
-		Path.Emplace(Hex);
-		Hex = Hex->CameFrom;
+	AHexTile* iPathNode = Goal;
+
+	while (iPathNode != Start) {
+		Path.Emplace(iPathNode);
+
+		if (iPathNode->Distance(iPathNode->CameFrom) > 1) {
+			bChainBroken = true;
+			UE_LOG(LogTemp, Warning, TEXT("Chain is broken in LinkPath function..."));
 		}
-	Algo::Reverse(Path);
-	*/
+
+		iPathNode = iPathNode->CameFrom;
+	}
+
 	return Path;
 }

AdventureMap.h

@@ -43,9 +43,13 @@ public:
 	UFUNCTION(BlueprintCallable, Category = "Runtime")
 		TArray<AHexTile*> Neighbors(AHexTile* OfHex);
 	UFUNCTION(BlueprintCallable, Category = "Runtime")
-		TArray<AHexTile*> FindPathAStar(AHexTile* Start, AHexTile* Goal);
-	//UFUNCTION(BlueprintCallable, Category = "Runtime")
-	//	TArray<AHexTile*> FindPathAStarPQ(AHexTile* Start, AHexTile* Goal);
+		TArray<AHexTile*> AStar(AHexTile* Start, AHexTile* Goal);
+	UFUNCTION(BlueprintCallable, Category = "Runtime")
+		TArray<AHexTile*> LinkPath(AHexTile* Start, AHexTile* Goal);
+
+	UPROPERTY(BlueprintReadOnly, Category = "debug")
+		bool bChainBroken;
+
 
 	// Player spawn section
 	UPROPERTY(VisibleAnywhere, BlueprintReadWrite)

AdventurePlayerController.cpp

@@ -37,7 +37,7 @@ void AAdventurePlayerController::AdvClick()
     if (IsValid(Hit.GetActor()))
     {
         AHexTile* HitHex = (AHexTile*)Hit.GetActor();
-        MapRef->FindPathAStar(CurrentHex, HitHex);
+        // MapRef->FindPathAStar(CurrentHex, HitHex);
         // UE_LOG(LogTemp, Warning, TEXT("%d"), HitHex->Index);
     }
 }

HexTile.h

@@ -47,10 +47,16 @@ public:
 	// Pathfinding
 	UPROPERTY(BlueprintReadWrite, Category = "Movement")
 		int32 MoveCost = 1;
-	UPROPERTY(VisibleInstanceOnly, Category = "Movement")
+	UPROPERTY(BlueprintReadWrite, VisibleInstanceOnly, Category = "Movement")
 		AHexTile* CameFrom;
 	UPROPERTY(VisibleInstanceOnly, Category = "Movement")
 		int32 CostSoFar = 0;
+	UPROPERTY()
+		int32 FCost;
+	UPROPERTY()
+		int32 GCost;
+	UPROPERTY()
+		int32 HCost;
 
 	FORCEINLINE bool operator == (const AHexTile &Other)
 	{