void findpath(Vector3 startPos, Vector3 targetpos) // used to find the path between two points
{
NodeScript startnode = Grid.NodefromWorldPoint(startPos);
NodeScript targetnode = Grid.NodefromWorldPoint(targetpos);
NodeScript MidPointnode = Grid.NodefromWorldPoint(midPoint);
List <NodeScript> openSet = new List <NodeScript>();
HashSet <NodeScript> closeSet = new HashSet <NodeScript>();
openSet.Add(startnode);
while (openSet.Count > 0)
{
NodeScript currentNode = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
if (Retries == 0 && openSet[i].fcost < currentNode.fcost || openSet[i].fcost == currentNode.fcost && openSet[i].hCost < currentNode.hCost) // checking to see if the next node has a lower cost, if it does make it the current node
{
currentNode = openSet[i];
}
allNodes.Add(currentNode);
}
openSet.Remove(currentNode); // when you move to the next node taske the cuuent node out of the open list so we cant go back to it
closeSet.Add(currentNode); // add to the closed list so we know its been tested;
if (currentNode == targetnode) // if we have hit out target then we are complete and leave the loop
{
reTracePath(startnode, targetnode);
return;
}
if (quickestTrack)
{
foreach (NodeScript Neighbour in Grid.GetNeighbours(currentNode))
{
if (!Neighbour.walkable || closeSet.Contains(Neighbour) || allNodes.Contains(Neighbour))
{
continue;
}
int newMovementCostToNeighb = currentNode.gCost + getDistance(currentNode, Neighbour);
if (newMovementCostToNeighb < Neighbour.gCost || !openSet.Contains(Neighbour)) // checking to see if the nieghbour has a shorter path then the others or that it is not in the open list
{ // if the neighbour is shorter then set its cost to the distance it is away from the target node
Neighbour.gCost = newMovementCostToNeighb;
Neighbour.hCost = getDistance(Neighbour, targetnode);
Neighbour.parent = currentNode;
if (!openSet.Contains(Neighbour))
{
openSet.Add(Neighbour);
}
}
}
}
if (hillyTrack)
{
foreach (NodeScript Neighbour in Grid.GetNeighbours(currentNode))
{
if (!Neighbour.walkable || closeSet.Contains(Neighbour))
{
continue;
}
int newMovementCostToNeighb = currentNode.gCost + getDistance(currentNode, Neighbour);
if (newMovementCostToNeighb < Neighbour.gCost || !openSet.Contains(Neighbour)) // checking to see if the nieghbour has a shorter path then the others or that it is not in the open list
{ // if the neighbour is shorter then set its cost to the distance it is away from the target node
if (Neighbour.worldPos.y < currentNode.worldPos.y)
{
currentNode.gCost += hillAmount;
HillMovementCostToNeighb = currentNode.gCost + getDistance(currentNode, Neighbour);
Neighbour.gCost = HillMovementCostToNeighb;
}
else
{
Neighbour.gCost = newMovementCostToNeighb;
}
Neighbour.hCost = getDistance(Neighbour, targetnode);
Neighbour.parent = currentNode;
if (!openSet.Contains(Neighbour))
{
openSet.Add(Neighbour);
}
}
}
}
}
}
