Fix working of the algorithm on enabled diagonal movement

Author: JasonNumberThirteen

Assets/Project/Scripts/Data Structures.meta

@@ -0,0 +1,15 @@
+public class MapTileNodeData
+{
+	public float TotalCost => RealValue + HeuristicValue;
+
+	public MapTileNode Parent {get; private set;}
+	public float RealValue {get; private set;}
+	public float HeuristicValue {get; private set;}
+
+	public void SetValues(MapTileNode parentMapTileNode, float realValue, float heuristicValue)
+	{
+		Parent = parentMapTileNode;
+		RealValue = realValue;
+		HeuristicValue = heuristicValue;
+	}
+}

Assets/Project/Scripts/Data Structures/MapTileNodeData.cs

@@ -45,7 +45,13 @@ public void ModifyWeightBy(int weight)
 		SetWeightTo(this.weight + weight);
 	}
 
-	public void SetWeightTo(int weight)
+	private void Awake()
+	{
+		mapTileNode = GetComponent<MapTileNode>();
+		visualiserEventsManager = FindFirstObjectByType<VisualiserEventsManager>();
+	}
+
+	private void SetWeightTo(int weight)
 	{
 		var previousWeight = this.weight;
 
@@ -66,10 +72,4 @@ public void SetWeightTo(int weight)
 			visualiserEventsManager.SendEvent(new MapTileBoolVisualiserEvent(this, VisualiserEventType.MapTileWeightWasChanged, true));
 		}
 	}
-
-	private void Awake()
-	{
-		mapTileNode = GetComponent<MapTileNode>();
-		visualiserEventsManager = FindFirstObjectByType<VisualiserEventsManager>();
-	}
 }

Assets/Project/Scripts/Data Structures/MapTileNodeData.cs.meta

@@ -6,30 +6,19 @@
 public class MapTileNode : MonoBehaviour
 {
 	public UnityEvent<MapTileNodeType> mapTileNodeTypeWasChangedEvent;
-	
+
 	public int Weight {get; set;}
-	public MapTileNode Parent {get; set;}
 
 	private MapTileNodeType mapTileNodeType;
 	private PathfindingManager pathfindingManager;
 
+	private readonly MapTileNodeData mapTileNodeData = new();
 	private readonly List<MapTileNode> neighbours = new();
 
 	public Vector2 GetPosition() => transform.position;
 	public List<MapTileNode> GetNeighbours() => neighbours;
 	public MapTileNodeType GetMapTileNodeType() => mapTileNodeType;
-
-	public float GetCostToReachTo(MapTileNode destinationMapTileNode)
-	{
-		if(destinationMapTileNode == null)
-		{
-			return float.MaxValue;
-		}
-
-		var heuristicValue = pathfindingManager != null ? pathfindingManager.GetHeuristicValue(GetPosition(), destinationMapTileNode.GetPosition()) : 0f;
-
-		return GetPathLengthFromStart() + heuristicValue;
-	}
+	public MapTileNodeData GetMapTileNodeData() => mapTileNodeData;
 
 	public void SetTileNodeType(MapTileNodeType mapTileNodeType)
 	{
@@ -48,8 +37,7 @@ public void SetTileNodeType(MapTileNodeType mapTileNodeType)
 	public void ResetData()
 	{
 		SetTileNodeType(MapTileNodeType.Unvisited);
-		
-		Parent = null;
+		mapTileNodeData.SetValues(null, 0, 0);
 	}
 
 	public void FindNeighbours(List<MapTileNode> mapTileNodes)
@@ -76,16 +64,4 @@ private void AddNeighbourIfPossible(List<MapTileNode> mapTileNodes, Vector2 dire
 			neighbours.Add(neighbouringNode);
 		}
 	}
-
-	private int GetPathLengthFromStart()
-	{
-		var length = 0;
-
-		if(pathfindingManager != null)
-		{
-			pathfindingManager.OperateOnMapTileNodes(this, mapTileNode => length += mapTileNode.Weight);
-		}
-
-		return length;
-	}
 }

Assets/Project/Scripts/Data Structures/PriorityQueue.cs

@@ -5,6 +5,8 @@ public class HeuristicManager : MonoBehaviour
 	private HeuristicType heuristicType;
 	private float heuristicWeight;
 
+	private static readonly float TIE_BREAKING_HEURISTIC_MULTIPLIER = 0.001f;
+
 	public void SetHeuristicType(HeuristicType heuristicType)
 	{
 		this.heuristicType = heuristicType;
@@ -17,6 +19,7 @@ public void SetHeuristicWeight(float weight)
 
 	public float GetHeuristicValue(Vector2 positionA, Vector2 positionB)
 	{
+		var heuristicMultiplier = 1 + TIE_BREAKING_HEURISTIC_MULTIPLIER;
 		var heuristicValue = heuristicType switch
 		{
 			HeuristicType.ManhattanDistance => DistanceMethods.GetOneDimensionalDistance(positionA.x, positionB.x) + DistanceMethods.GetOneDimensionalDistance(positionA.y, positionB.y),
@@ -25,6 +28,6 @@ public float GetHeuristicValue(Vector2 positionA, Vector2 positionB)
 			_ => 0f
 		};
 
-		return heuristicValue*heuristicWeight;
+		return heuristicValue*heuristicWeight*heuristicMultiplier;
 	}
 }

Assets/Project/Scripts/Data Structures/PriorityQueue.cs.meta

@@ -1,4 +1,3 @@
-using System;
 using System.Linq;
 using System.Collections;
 using System.Collections.Generic;
@@ -14,8 +13,8 @@ public class PathfindingManager : MonoBehaviour
 
 	private readonly List<MapTile> mapTilesInScene = new();
 	private readonly List<MapTileNode> pathMapTileNodes = new();
+	private readonly List<MapTileNode> mapTileNodesWaitingForVisit = new();
 	private readonly List<IMapEditingElement> mapEditingElements = new();
-	private readonly PriorityQueue<MapTileNode> priorityQueue = new();
 
 	private MapTile startMapTile;
 	private MapTile destinationMapTile;
@@ -59,7 +58,6 @@ private bool PathfindingWasStarted
 		}
 	}
 
-	public float GetHeuristicValue(Vector2 positionA, Vector2 positionB) => heuristicManager != null ? heuristicManager.GetHeuristicValue(positionA, positionB) : 0f;
 	public bool DiagonalMovementIsEnabled() => diagonalMovementIsEnabled;
 
 	public void FindPath()
@@ -86,18 +84,6 @@ public void ClearResults()
 		resultsWereClearedEvent?.Invoke();
 	}
 
-	public void OperateOnMapTileNodes(MapTileNode mapTileNodeToStartFrom, Action<MapTileNode> action)
-	{
-		var currentMapTileNode = mapTileNodeToStartFrom;
-
-		while (currentMapTileNode.Parent != null)
-		{
-			action?.Invoke(currentMapTileNode);
-
-			currentMapTileNode = currentMapTileNode.Parent;
-		}	
-	}
-
 	public void SetDiagonalMovementEnabled(bool enabled)
 	{
 		diagonalMovementIsEnabled = enabled;
@@ -202,7 +188,7 @@ private void ClearData()
 		destinationMapTile = mapTilesInScene.FirstOrDefault(mapTile => mapTile.GetTileType() == MapTileType.Destination);
 
 		ClearResults();
-		priorityQueue.Clear();
+		mapTileNodesWaitingForVisit.Clear();
 		CreateConnectionsBetweenMapTileNodes();
 	}
 
@@ -221,19 +207,28 @@ private void InitiatePathfinder()
 		}
 
 		mapTilesInScene.ForEach(mapTile => mapTile.GetMapTileNode().ResetData());
-		startMapTile.SetWeightTo(0);
-		AddMapTileNodeToQueue(startMapTile.GetMapTileNode());
+		InitiateStartMapTile();
 
 		PathWasFound = false;
 	}
 
+	private void InitiateStartMapTile()
+	{
+		if(startMapTile != null)
+		{
+			mapTileNodesWaitingForVisit.Add(startMapTile.GetMapTileNode());
+		}
+	}
+
 	private IEnumerator FindPathToDestination()
 	{
 		var simulationIsEnabled = simulationManager != null && simulationManager.SimulationIsEnabled();
 
-		while (!pathWasFound && priorityQueue.Count > 0)
+		while (!pathWasFound && mapTileNodesWaitingForVisit.Count > 0)
 		{
-			if(VisitMapTileNodeIfNeeded(priorityQueue.Dequeue()) && simulationIsEnabled)
+			var currentMapTileNode = mapTileNodesWaitingForVisit.OrderBy(mapTileNode => mapTileNode.GetMapTileNodeData().TotalCost).FirstOrDefault();
+
+			if(VisitMapTileNodeIfPossible(currentMapTileNode) && simulationIsEnabled)
 			{
 				yield return simulationManager.GetNextStepDelayDependingOnSimulationType();
 			}
@@ -242,16 +237,17 @@ private IEnumerator FindPathToDestination()
 		PathfindingWasStarted = false;
 	}
 
-	private bool VisitMapTileNodeIfNeeded(MapTileNode mapTileNode)
+	private bool VisitMapTileNodeIfPossible(MapTileNode mapTileNode)
 	{
-		if(mapTileNode == null || mapTileNode.GetMapTileNodeType() == MapTileNodeType.Visited || destinationMapTile == null)
+		if(mapTileNode == null)
 		{
 			return false;
 		}
-
+		
 		mapTileNode.SetTileNodeType(MapTileNodeType.Visited);
-		OperateOnMapTileNode(mapTileNode);
+		mapTileNodesWaitingForVisit.Remove(mapTileNode);
 		mapTileNodeWasVisitedEvent?.Invoke(mapTileNode);
+		OperateOnMapTileNode(mapTileNode);
 
 		return true;
 	}
@@ -269,7 +265,7 @@ private void OperateOnMapTileNode(MapTileNode mapTileNode)
 		}
 		else
 		{
-			AddNeighboursOf(mapTileNode);
+			OperateOnNeighboursOf(mapTileNode);
 		}
 	}
 
@@ -282,40 +278,66 @@ private void FinishSearchingOn(MapTileNode mapTileNode)
 
 		PathWasFound = true;
 
-		OperateOnMapTileNodes(mapTileNode, mapTileNode => pathMapTileNodes.Add(mapTileNode));
+		DefinePathMapTileNodes(mapTileNode);
 		pathMapTileNodes.ForEach(mapTileNode => mapTileNode.SetTileNodeType(MapTileNodeType.BelongingToPath));
 		pathWasFoundEvent?.Invoke(pathMapTileNodes);
 	}
 
-	private void AddNeighboursOf(MapTileNode mapTileNode)
+	private void DefinePathMapTileNodes(MapTileNode mapTileNodeToStartFrom)
 	{
-		if(mapTileNode == null)
-		{
-			return;
-		}
+		var currentMapTileNode = mapTileNodeToStartFrom;
+		var currentMapTileNodeData = currentMapTileNode.GetMapTileNodeData();
 
-		var neighbours = mapTileNode.GetNeighbours().Where(neighbour => neighbour.GetMapTileNodeType() != MapTileNodeType.Visited).ToList();
+		while (currentMapTileNodeData.Parent != null)
+		{
+			pathMapTileNodes.Add(currentMapTileNode);
 
-		neighbours?.ForEach(neighbour => SetupAndAddNeighbour(neighbour, mapTileNode));
+			currentMapTileNode = currentMapTileNodeData.Parent;
+			currentMapTileNodeData = currentMapTileNode.GetMapTileNodeData();
+		}	
 	}
 
-	private void SetupAndAddNeighbour(MapTileNode neighbouringMapTile, MapTileNode parentMapTile)
+	private void OperateOnNeighboursOf(MapTileNode parentMapTileNode)
 	{
-		neighbouringMapTile.Parent = parentMapTile;
+		if(parentMapTileNode == null)
+		{
+			return;
+		}
+		
+		var neighbours = parentMapTileNode.GetNeighbours().Where(neighbour => neighbour.GetMapTileNodeType() != MapTileNodeType.Visited).ToList();
 
-		AddMapTileNodeToQueue(neighbouringMapTile);
+		neighbours.ForEach(neighbour => OperateOnNeighbourIfNeeded(parentMapTileNode, neighbour));
 	}
 
-	private void AddMapTileNodeToQueue(MapTileNode mapTileNode)
+	private void OperateOnNeighbourIfNeeded(MapTileNode parentMapTileNode, MapTileNode neighbouringMapTileNode)
 	{
-		if(mapTileNode == null)
+		if(parentMapTileNode == null || neighbouringMapTileNode == null)
 		{
 			return;
 		}
+		
+		var totalCostToReachNeighbourFromParent = GetTotalCostToReachNeighbourFromParent(parentMapTileNode, neighbouringMapTileNode);
+		var neighbourIsAlreadyWaitingForVisit = mapTileNodesWaitingForVisit.Contains(neighbouringMapTileNode);
+		var neighbourMapTileNodeData = neighbouringMapTileNode.GetMapTileNodeData();
 
-		var mapTileNodeCost = mapTileNode.GetCostToReachTo(destinationMapTile.GetMapTileNode());
-		var mapTileNodeWithCost = new PriorityQueueElement<MapTileNode>(mapTileNode, mapTileNodeCost);
+		if(neighbourIsAlreadyWaitingForVisit && totalCostToReachNeighbourFromParent >= neighbourMapTileNodeData.RealValue)
+		{
+			return;
+		}
+
+		neighbourMapTileNodeData.SetValues(parentMapTileNode, totalCostToReachNeighbourFromParent, heuristicManager != null ? heuristicManager.GetHeuristicValue(neighbouringMapTileNode.GetPosition(), destinationMapTile.GetMapTileNode().GetPosition()) : 0f);
+
+		if(!neighbourIsAlreadyWaitingForVisit)
+		{
+			mapTileNodesWaitingForVisit.Add(neighbouringMapTileNode);
+		}
+	}
 
-		priorityQueue.Enqueue(mapTileNodeWithCost);
+	private float GetTotalCostToReachNeighbourFromParent(MapTileNode parentMapTileNode, MapTileNode neighbouringMapTileNode)
+	{
+		var distanceToNeighbour = DistanceMethods.DistanceBetweenPositionsIsSingleAxis(parentMapTileNode.GetPosition(), neighbouringMapTileNode.GetPosition()) ? 1 : Mathf.Sqrt(2);
+		var neighbourRealValue = distanceToNeighbour*neighbouringMapTileNode.Weight;
+		
+		return parentMapTileNode.GetMapTileNodeData().RealValue + neighbourRealValue;
 	}
 }

Assets/Project/Scripts/Game Objects/Map Tile/MapTile.cs

@@ -2,5 +2,14 @@
 
 public static class DistanceMethods
 {
+	public static bool OneDimensionalDistanceBetweenPositionsIsSingleAxis(float a, float b) => Mathf.Approximately(GetOneDimensionalDistance(a, b), 0f);
 	public static float GetOneDimensionalDistance(float a, float b) => Mathf.Abs(a - b);
+	
+	public static bool DistanceBetweenPositionsIsSingleAxis(Vector2 positionA, Vector2 positionB)
+	{
+		var distanceInXIsSingleAxis = OneDimensionalDistanceBetweenPositionsIsSingleAxis(positionA.x, positionB.x);
+		var distanceInYIsSingleAxis = OneDimensionalDistanceBetweenPositionsIsSingleAxis(positionA.y, positionB.y);
+		
+		return distanceInXIsSingleAxis || distanceInYIsSingleAxis;
+	}
 }