JPS Pathfinding Algorithm

JPSPathfinding.cpp

[System.Serializable]
public class Pathfinding_JSP : PathFinding
{
	bool ortoganolFound;

	public bool manhatten = true, eucliden, octile, chebyshev;
	
	[System.Serializable]
	class NodeInformation
	{
		public GridNode node;
		public NodeInformation parent;
		public float gCost;
		public float hCost;
		public float fCost;

		public NodeInformation(GridNode node, NodeInformation parent, float gCost, float hCost)
		{
			this.node = node;
			this.parent = parent;
			this.gCost = gCost;
			this.hCost = hCost;
			fCost = gCost + hCost;
		}

		public void UpdateNodeInformation(NodeInformation parent, float gCost, float hCost)
		{
			this.parent = parent;
			this.gCost = gCost;
			this.hCost = hCost;
			fCost = gCost + hCost;
		}
	}

	public Pathfinding_JSP(bool allowDiagonal, bool cutCorners) : base(allowDiagonal, cutCorners) { }

	public override void GeneratePath(GridNode start, GridNode end)
	{
		//clears the current path
		m_Path.Clear();

		//lists to track visited and none visited nodes
		List<NodeInformation> openList = new List<NodeInformation>();
		List<NodeInformation> closedList = new List<NodeInformation>();
		//forcedNeighbours


		//starting node
		NodeInformation startingNode = new NodeInformation(start, null, 0, 0);
		openList.Add(startingNode);

		NodeInformation current = startingNode;

		int maxIteration = 0;

		while (current != null)
		{
			maxIteration++;
			if (maxIteration > m_MaxPathCount)
			{
				Debug.LogError("Max iteration has been reached!!");
				break;
			}

			if (current.node == end)
			{
				Debug.Log("Path found, start pos: " + start.transform.position + " - end pos: " + end.transform.position);
				SetPath(current);
				DrawPath(closedList, openList);
				return;
			}

			SearchOrthagonal(current, end, openList, closedList);
			SearchDiagonal(current, end, openList, closedList);

			closedList.Add(current);
			openList.Remove(current);

			if (openList.Count > 0)
			{
				current = GetCheapestNode(openList);
			}
			else break;



		}


		Debug.LogError("No path found, start pos = " + start.transform.position + " - end pos = " + end.transform.position);
	}

	/// <summary>
	/// pass in the final node information and sets m_Path
	/// </summary>
	private void SetPath(NodeInformation end)
	{
		NodeInformation current = end;
		while (current != null)
		{
			m_Path.Add(current.node.transform.position);
			current = current.parent;
		}

		m_Path.Reverse();
	}

	/// <summary>
	/// Returns the cheapest node in the list calculated by cost
	/// </summary>
	private NodeInformation GetCheapestNode(List<NodeInformation> nodes)
	{
		return nodes.OrderBy(n => n.fCost).First();
	}

	/// <summary>
	/// checks if a grid node reference is held within a list of Node Informations
	/// </summary>
	bool DoesListContainNode(List<NodeInformation> nodeInformation, GridNode gridNode)
	{
		return nodeInformation.Any(x => x.node == gridNode);
	}

	/// <summary>
	/// Returns a Node Information if a grid node reference is within the list
	/// </summary>
	NodeInformation GetNodeInformationFromList(List<NodeInformation> nodeInformation, GridNode gridNode)
	{
		return nodeInformation.Find(x => x.node == gridNode);
	}

	/// <summary>
	/// returns the next node in a set direction
	/// </summary>
	GridNode GetNeighbourInDirection(GridNode current, int direction)
	{
		return current.Neighbours[direction];
	}

	/// <summary>
	/// returns a node either clockwise or anticlockwise in a set direction
	/// </summary>
	GridNode GetDiagonalNeighbours(GridNode current, int direction, bool clockwise)
	{
		int modifier = direction;

		if (clockwise) { modifier++; }
		else { modifier--; }

		int neighbour = (modifier % 8 + 8) % 8;

		return current.Neighbours[neighbour];
	}

	/// <summary>
	/// Changest the colour of the grid based on the values passed in
	/// </summary>
	void DrawPath(List<NodeInformation> open, List<NodeInformation> closed)
	{
		//drawPath
		if (m_Debug_ChangeTileColours)
		{
			Grid.ResetGridNodeColours();

			foreach (NodeInformation node in closed)
			{
				node.node.SetOpenInPathFinding();
			}

			foreach (NodeInformation node in open)
			{
				node.node.SetClosedInPathFinding();
			}
		}
	}

	void SearchOrthagonal(NodeInformation current, GridNode end, List<NodeInformation> openList, List<NodeInformation> closedList, int diagonalStep = 0)
	{
		ortoganolFound = false;
        for (int i = 0; i < 8; i++)
		{
			GridNode tempNode = current.node;
			float tempGCost = current.gCost;
			//directionalmovement
			if (i % 2 != 0) continue;
			//if (i != 0 || i != GetDirection(diagonalStep - 1) || i != GetDirection(diagonalStep + 1)) continue;

			GridNode neighbour = GetNeighbourInDirection(current.node, i);
			while (neighbour != null && neighbour.m_Walkable)
			{
				float distance = Maths.Magnitude(neighbour.transform.position - tempNode.transform.position);
				float tempHCost = CalculateHeuristic(neighbour, end);
				tempGCost += distance;

				if (neighbour == end)
				{
					NodeInformation newNode = new NodeInformation(neighbour, current, tempGCost, tempHCost);
					openList.Add(newNode);
					break;
				}

				if (!GetNeighbourInDirection(neighbour, GetDirection(i - 2)).m_Walkable && GetNeighbourInDirection(neighbour, GetDirection(i - 1)).m_Walkable || !GetNeighbourInDirection(neighbour, GetDirection(i + 2)).m_Walkable && GetNeighbourInDirection(neighbour, GetDirection(i + 1)).m_Walkable)
				{
                    NodeInformation newNode = new NodeInformation(neighbour, current, tempGCost, tempHCost);

					AddNode(newNode, openList, closedList);
                    ortoganolFound = true;                    
				}       
				tempNode = neighbour;
                neighbour = GetNeighbourInDirection(neighbour, i);
			}
		}		
	}

	void SearchDiagonal(NodeInformation current, GridNode end, List<NodeInformation> openList, List<NodeInformation> closedList)
	{

		for (int i = 0; i < 8; i++)
		{
			GridNode tempNode = current.node;			
			float tempGCost = current.gCost;

			if (i % 2 == 0) continue;

			GridNode neighbour = GetNeighbourInDirection(current.node, i);
			while (neighbour != null && neighbour.m_Walkable)
			{

				//check if wall or jump point

				float distance = Maths.Magnitude(neighbour.transform.position - tempNode.transform.position);				
                float tempHCost = CalculateHeuristic(neighbour, end);
				tempGCost += distance;

				if (neighbour == end)
				{
					NodeInformation newNode = new NodeInformation(neighbour, current, tempGCost, tempHCost);
					openList.Add(newNode);
					break;
				}

				NodeInformation checkNeighbour = new NodeInformation(neighbour, current, tempGCost, tempHCost);
				SearchOrthagonal(checkNeighbour, end, openList, closedList, i);
				if (ortoganolFound)
				{
					AddNode(checkNeighbour, openList, closedList);
					ortoganolFound = false;
					break;
				}

				if ((!GetNeighbourInDirection(neighbour, GetDirection(i - 2)).m_Walkable && GetNeighbourInDirection(neighbour, GetDirection(i - 1)).m_Walkable) || (!GetNeighbourInDirection(neighbour, GetDirection(i + 2)).m_Walkable && GetNeighbourInDirection(neighbour, GetDirection(i + 1)).m_Walkable))
				{
					NodeInformation newNode = new NodeInformation(neighbour, current, tempGCost, tempHCost);

					AddNode(newNode, openList, closedList);					

					break;
				}
				tempNode = neighbour;
				neighbour = GetNeighbourInDirection(neighbour, i);
			}
		}
	}


	int GetDirection(int curDirection)
	{
		while (curDirection > 7)
		{
			curDirection -= 8;
		}
		while (curDirection < 0)
		{
			curDirection += 8;
		}

		return curDirection;
	}

	void AddNode(NodeInformation nodeToAdd, List<NodeInformation> openList, List<NodeInformation> closedList)
	{		
		if (DoesListContainNode(openList, nodeToAdd.node))
		{
			if (GetNodeInformationFromList(openList, nodeToAdd.node).fCost > nodeToAdd.fCost)
			{
				GetNodeInformationFromList(openList, nodeToAdd.node).UpdateNodeInformation(nodeToAdd.parent, nodeToAdd.gCost, nodeToAdd.hCost);
			}
		}
		else if (DoesListContainNode(closedList, nodeToAdd.node))
		{
			if (GetNodeInformationFromList(closedList, nodeToAdd.node).fCost > nodeToAdd.fCost)
			{
				GetNodeInformationFromList(closedList, nodeToAdd.node).UpdateNodeInformation(nodeToAdd.parent, nodeToAdd.gCost, nodeToAdd.hCost);
			}
		}
		else
		{
			openList.Add(nodeToAdd);
		}
	}

	float CalculateHeuristic(GridNode start, GridNode end)
	{
		if (manhatten) return Heuristic_Manhattan(start, end);
		else if (eucliden) return Heuristic_Euclidean(start, end);
		else if (octile) return Heuristic_Octile(start, end);
		else if (chebyshev) return Heuristic_Chebyshev(start, end);
		else return Heuristic_Manhattan(start, end);

    }
}