Hello,
I am trying to create an A* pathfinding algorithm using waypoints which I have generated using a separate script on my terrain.
Traversable and untraversable waypoints on the terrain are defined by their colour - Green being traversable.
The traversable waypoints are stored in a list.
To start the pathfinding alogrithm I created a dictionary which stored each traversable waypoint as a unique key (type = gameobject). The values of each key were its neighbours (type = List) calculated using the distance function.
I then tried to create an A* algorithm using online references and adapting these to my situation. The comments to each part can be seen in my code below. The function ‘findPath’ is the actual algorithm (well, my best attempt at it).
void findPath()
{
// Adds start node to open list
// take nodes around start node and add to lista*
open.Add(startNode);
while (open.Count > 0)
{
nodeDistances = 1000;
foreach (GameObject node in open)
{
GCost = Vector3.Distance(startNode.transform.position, node.transform.position);// G distance form node to start
HCost = Vector3.Distance(targetNode.transform.position, node.transform.position); // H distance from node to target
FCost = GCost + HCost;
if (FCost < nodeDistances) // if current node has smaller F cost than the node before that
{
tempGCost = GCost; // Gets the lowest G cost
tempFCost = FCost;
current = node; // Replaces Game Object variable
nodeDistances = FCost;
}
}
if (current.transform.position == targetNode.transform.position)
{
Debug.Log("Goal reached");
break;
}
open.Remove(current);
closed.Add(current);
neighbours = attachedToWaypoint[current];
// path.Add(current);
foreach (GameObject n in neighbours)
{
if (!closed.Contains(n))
{
float g_score = tempGCost + 1; // Takes current node GCost and adds value of 1 for each neighbour
float h_score = Vector3.Distance(n.transform.position, targetNode.transform.position); // Gets distance from each neighbour to the target node
float f_score = g_score + h_score;
if (closed.Contains(n) && f_score >= tempFCost) // If F cost of neighbour is greater than F cost of current node
continue;
if (!open.Contains(n) || f_score < tempFCost) // If 'open' list does not currently contain neighbour or neighbours F score is smaller than that of the current node
{
GCost = g_score;
HCost = h_score;
tempFCost = GCost + HCost; // update current node F score value to get neighbour with the smallest F score
if (!open.Contains(n))
{
open.Add(n); // if neihgbour is not in open list, add to open list
}
}
}
}
}
}
However, looking at the nodes stored in the closed list, it is evident that something is going wrong - pic below of nodes within the closed list:
Could someone with more experience please give me some pointers on what aspect I should focus on? Additionally, how would you use this to find the cheapest path?
Any help would be extremely appreciated.
Thanks,
Calvin
Full script code below:
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class TEST : MonoBehaviour
{
Dictionary<GameObject, List<GameObject>> attachedToWaypoint = new Dictionary<GameObject, List<GameObject>>();
List<GameObject> gameObjectForDic = new List<GameObject>();
GameObject[] waypoints;
List<List<GameObject>> nodeData = new List<List<GameObject>>();
List<GameObject> neighbours = new List<GameObject>(); // Not currently used
public GameObject enemy;
public GameObject player;
GameObject startNode;
GameObject targetNode;
List<GameObject> open = new List<GameObject>();
List<GameObject> closed = new List<GameObject>();
float distanceToEnemy;
float distanceToPlayer;
float tempFCost;
float FCost;
float GCost;
float HCost;
float tempGCost;
float accumulatedCost;
float accumulatedCostTemp;
float nodeDistances;
GameObject current;
public GameObject parent;
List<GameObject> path = new List<GameObject>();
// Use this for initialization
void Start()
{
distanceToEnemy = 1000;
distanceToPlayer = 1000;
nodeDistances = 10000;
waypoints = GameObject.FindGameObjectsWithTag("Waypoint");
storeNodesInDictionary();
findPath();
// findPathtoPlayer();
testing();
}
void storeNodesInDictionary()
{
for (int i = 0; i < waypoints.Length; i++)
{
nodeData.Add(new List<GameObject>());
float distEnemy = Vector3.Distance(enemy.transform.position, waypoints[i].transform.position); // Closest node to enemy
if (distEnemy < distanceToEnemy)
{
startNode = waypoints[i];
distanceToEnemy = distEnemy;
}
float distPlayer = Vector3.Distance(player.transform.position, waypoints[i].transform.position);// Closest node to player
if (distPlayer < distanceToPlayer)
{
targetNode = waypoints[i];
distanceToPlayer = distPlayer;
}
for (int j = 0; j < waypoints.Length; j++)
{
float distanceSqr = (waypoints[i].transform.position - waypoints[j].transform.position).sqrMagnitude; // Gets distance values
if (distanceSqr < 60 && waypoints[i] != waypoints[j]) // Is waypoints are within a spcific distance
{
nodeData[i].Add(waypoints[j]);
}
}
attachedToWaypoint.Add(waypoints[i], nodeData[i]); // Adds parent node and neighbouring nodes within a 3x3 grid
}
}
void findPath()
{
// Adds start node to open list
// take nodes around start node and add to lista*
open.Add(startNode);
while (open.Count > 0)
{
nodeDistances = 1000;
foreach (GameObject node in open)
{
GCost = Vector3.Distance(startNode.transform.position, node.transform.position);// G distance form node to start
HCost = Vector3.Distance(targetNode.transform.position, node.transform.position); // H distance from node to target
FCost = GCost + HCost;
if (FCost < nodeDistances) // if current node has smaller F cost than the node before that
{
tempGCost = GCost; // Gets the lowest G cost
tempFCost = FCost;
current = node; // Replaces Game Object variable
nodeDistances = FCost;
}
}
if (current.transform.position == targetNode.transform.position)
{
Debug.Log("Goal reached");
break;
}
open.Remove(current);
closed.Add(current);
neighbours = attachedToWaypoint[current];
// path.Add(current);
foreach (GameObject n in neighbours)
{
if (!closed.Contains(n))
{
float g_score = tempGCost + 1; // Takes current node GCost and adds value of 1 for each neighbour
float h_score = Vector3.Distance(n.transform.position, targetNode.transform.position); // Gets distance from each neighbour to the target node
float f_score = g_score + h_score;
if (closed.Contains(n) && f_score >= tempFCost) // If F cost of neighbour is greater than F cost of current node
continue;
if (!open.Contains(n) || f_score < tempFCost) // If 'open' list does not currently contain neighbour or neighbours F score is smaller than that of the current node
{
GCost = g_score;
HCost = h_score;
tempFCost = GCost + HCost; // update current node F score value to get neighbour with the smallest F score
if (!open.Contains(n))
{
open.Add(n); // if neihgbour is not in open list, add to open list
}
}
}
}
}
}
}