int GetManhattenDistance(SimplifiedNode a_nodeA, SimplifiedNode a_nodeB)
{
int ix = Mathf.Abs(a_nodeA.iGridX - a_nodeB.iGridX); //x1-x2
int iy = Mathf.Abs(a_nodeA.iGridY - a_nodeB.iGridY); //y1-y2
return(ix + iy); //Return the sum
}
//Find path function for Iohannis
public void iFindPath(Vector3 a_StartPos, Vector3 a_TargetPos)
{
//Vector3[] direction = new Vector3[0];
SimplifiedNode StartNode = GridReference.NodeFromWorldPoint(a_StartPos); //Gets the node closest to the starting position
SimplifiedNode TargetNode = GridReference.NodeFromWorldPoint(a_TargetPos); //Gets the node closest to the target position
List <SimplifiedNode> OpenList = new List <SimplifiedNode>(); //List of nodes for the open list
HashSet <SimplifiedNode> ClosedList = new HashSet <SimplifiedNode>(); //Hashset of nodes for the closed list
OpenList.Add(StartNode); //Add the starting node to the open list to begin the program
while (OpenList.Count > 0) //Whilst there is something in the open list
{
SimplifiedNode CurrentNode = OpenList[0]; //Create a node and set it to the first item in the open list
for (int i = 1; i < OpenList.Count; i++) //Loop through the open list starting from the second object
{
if (OpenList[i].FCost < CurrentNode.FCost || OpenList[i].FCost == CurrentNode.FCost && OpenList[i].ihCost < CurrentNode.ihCost) //If the f cost of that object is less than or equal to the f cost of the current node
{
CurrentNode = OpenList[i]; //Set the current node to that object
}
}
OpenList.Remove(CurrentNode); //Remove that from the open list
ClosedList.Add(CurrentNode); //And add it to the closed list
if (CurrentNode == TargetNode) //If the current node is the same as the target node
{
iGetFinalPath(StartNode, TargetNode); //Calculate the final path
// veorica.GetComponent<Veorica>().nrPath++;
// veorica.GetComponent<Veorica>().direction = RetracePath(StartNode, TargetNode);
}
foreach (SimplifiedNode NeighborNode in GridReference.GetNeighboringNodes(CurrentNode)) //Loop through each neighbor of the current node
{
if (!NeighborNode.bIsWall || ClosedList.Contains(NeighborNode)) //If the neighbor is a wall or has already been checked
{
continue; //Skip it
}
int MoveCost = CurrentNode.igCost + GetManhattenDistance(CurrentNode, NeighborNode); //Get the F cost of that neighbor
if (MoveCost < NeighborNode.igCost || !OpenList.Contains(NeighborNode)) //If the f cost is greater than the g cost or it is not in the open list
{
NeighborNode.igCost = MoveCost; //Set the g cost to the f cost
NeighborNode.ihCost = GetManhattenDistance(NeighborNode, TargetNode); //Set the h cost
NeighborNode.ParentNode = CurrentNode; //Set the parent of the node for retracing steps
if (!OpenList.Contains(NeighborNode)) //If the neighbor is not in the openlist
{
OpenList.Add(NeighborNode); //Add it to the list
}
else
{
ClosedList.Add(NeighborNode);
}
}
}
}
}
public Vector3[] RetracePath(SimplifiedNode startNode, SimplifiedNode endNode)
{
List <SimplifiedNode> path = new List <SimplifiedNode>();
SimplifiedNode currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.ParentNode;
}
Vector3[] waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return(waypoints);
}
//Function that gets the neighboring nodes of the given node.
public List <SimplifiedNode> GetNeighboringNodes(SimplifiedNode a_NeighborNode)
{
List <SimplifiedNode> NeighborList = new List <SimplifiedNode>(); //Make a new list of all available neighbors.
int icheckX; //Variable to check if the XPosition is within range of the node array to avoid out of range errors.
int icheckY; //Variable to check if the YPosition is within range of the node array to avoid out of range errors.
//Check the right side of the current node.
icheckX = a_NeighborNode.iGridX + 1;
icheckY = a_NeighborNode.iGridY;
if (icheckX >= 0 && icheckX < iGridSizeX) //If the XPosition is in range of the array
{
if (icheckY >= 0 && icheckY < iGridSizeY) //If the YPosition is in range of the array
{
NeighborList.Add(NodeArray[icheckX, icheckY]); //Add the grid to the available neighbors list
}
}
//Check the Left side of the current node.
icheckX = a_NeighborNode.iGridX - 1;
icheckY = a_NeighborNode.iGridY;
if (icheckX >= 0 && icheckX < iGridSizeX) //If the XPosition is in range of the array
{
if (icheckY >= 0 && icheckY < iGridSizeY) //If the YPosition is in range of the array
{
NeighborList.Add(NodeArray[icheckX, icheckY]); //Add the grid to the available neighbors list
}
}
//Check the Top side of the current node.
icheckX = a_NeighborNode.iGridX;
icheckY = a_NeighborNode.iGridY + 1;
if (icheckX >= 0 && icheckX < iGridSizeX) //If the XPosition is in range of the array
{
if (icheckY >= 0 && icheckY < iGridSizeY) //If the YPosition is in range of the array
{
NeighborList.Add(NodeArray[icheckX, icheckY]); //Add the grid to the available neighbors list
}
}
//Check the Bottom side of the current node.
icheckX = a_NeighborNode.iGridX;
icheckY = a_NeighborNode.iGridY - 1;
if (icheckX >= 0 && icheckX < iGridSizeX) //If the XPosition is in range of the array
{
if (icheckY >= 0 && icheckY < iGridSizeY) //If the YPosition is in range of the array
{
NeighborList.Add(NodeArray[icheckX, icheckY]); //Add the grid to the available neighbors list
}
}
return(NeighborList);//Return the neighbors list.
}
// get final path function for iohannis
void iGetFinalPath(SimplifiedNode a_StartingNode, SimplifiedNode a_EndNode)
{
List <SimplifiedNode> FinalPath = new List <SimplifiedNode>(); //List to hold the path sequentially
SimplifiedNode CurrentNode = a_EndNode; //Node to store the current node being checked
while (CurrentNode != a_StartingNode) //While loop to work through each node going through the parents to the beginning of the path
{
FinalPath.Add(CurrentNode); //Add that node to the final path
CurrentNode = CurrentNode.ParentNode; //Move onto its parent node
}
if (CurrentNode == a_StartingNode)
{
FinalPath.Add(CurrentNode);
}
FinalPath.Reverse(); //Reverse the path to get the correct order
iohannis.GetComponent <Iohannis>().iFinalPath = FinalPath; //Set the final path
// Debug.LogError("The final path assigned is: " + FinalPath);
// veorica.GetComponent<Veorica>().FinalPath[veorica.GetComponent<Veorica>().nrPath] = FinalPath;
}
void GetFinalPath(SimplifiedNode a_StartingNode, SimplifiedNode a_EndNode)
{
List <SimplifiedNode> FinalPath = new List <SimplifiedNode>(); //List to hold the path sequentially
SimplifiedNode CurrentNode = a_EndNode; //Node to store the current node being checked
while (CurrentNode != a_StartingNode) //While loop to work through each node going through the parents to the beginning of the path
{
FinalPath.Add(CurrentNode); //Add that node to the final path
CurrentNode = CurrentNode.ParentNode; //Move onto its parent node
}
if (CurrentNode == a_StartingNode)
{
FinalPath.Add(CurrentNode);
}
FinalPath.Reverse();//Reverse the path to get the correct order
// veorica.GetComponent<Veorica>().vFinalPath = FinalPath;//Set the final path
//uncomment this to use for pathfinding scene
GridReference.FinalPath = FinalPath;
}
void CreateGrid()
{
NodeArray = new SimplifiedNode[iGridSizeX, iGridSizeY]; //Declare the array of nodes.
Vector3 bottomLeft = transform.position - Vector3.right * vGridWorldSize.x / 2 - Vector3.forward * vGridWorldSize.y / 2; //Get the real world position of the bottom left of the grid.
for (int x = 0; x < iGridSizeX; x++) //Loop through the array of nodes.
{
for (int y = 0; y < iGridSizeY; y++) //Loop through the array of nodes
{
Vector3 worldPoint = bottomLeft + Vector3.right * (x * fNodeDiameter + fNodeRadius) + Vector3.forward * (y * fNodeDiameter + fNodeRadius); //Get the world co ordinates of the bottom left of the graph
bool Wall = true; //Make the node a wall
//If the node is not being obstructed
//Quick collision check against the current node and anything in the world at its position. If it is colliding with an object with a WallMask,
//The if statement will return false.
if (Physics.CheckSphere(worldPoint, fNodeRadius, WallMask))
{
Wall = false;//Object is not a wall
}
NodeArray[x, y] = new SimplifiedNode(Wall, worldPoint, x, y);//Create a new node in the array.
}
}
}
