/* getNodeOnList returns node from list that contains a specified polygon
* Parameter: (AIPolygon) polygonToCheck is the polygon being held by the node to be returned
* Return: (AIAgentAStarSearchNode)
* node on list that is holding polygonToCheck
*/
public AIAgentAStarSearchNode getNodeOnList(AIPolygon polygonToCheck)
{
AIAgentAStarSearchNode tempFront = frontOfList;
if (tempFront == null)
{
return(null);
}
if (doesIDMatch(polygonToCheck.getID(), tempFront.getPolygon().getID()) == true)
{
return(tempFront);
}
AIAgentAStarSearchNode tempBack = tempFront.getNextNode();
while ((tempBack != null) && (tempBack.getNextNode() != null) && (doesIDMatch(polygonToCheck.getID(), tempBack.getPolygon().getID()) == false))
{
tempFront = tempBack;
tempBack = tempFront.getNextNode();
}
if (tempBack == null)
{
return(null);
}
if (doesIDMatch(polygonToCheck.getID(), tempBack.getPolygon().getID()) == true)
{
return(tempBack);
}
return(null);
}
/*
* doSearch method will run the A* search till a goal is found, or a node is in another agent closed list, or the
* nodes expanded is equal to the nodesToExpand variable
* Parameter: (int)nodesToExpand is the number of nodes that the search can expand
* (AIAgentAStarSearchList) secondSearchClosedList is the closed list from the other agent
* Return: (int)
* 0 if nothing was found
* 1 if the goal for this search agent was found
* 2 if a node on the second agent's closed list was found
*/
public int doSearch(int nodesToExpand, AIAgentAStarSearchList secondSearchClosedList)
{
int nodesExpandedCount = 0;
while (openList.isEmpty() == false && nodesExpandedCount < nodesToExpand) //goes until nothing is left on the open list meaning a path could not be found
{
currentNode = openList.popNode(); //take the first(Best) polygon off the openList
queueSize--;
nodesVisited++;
if (currentNode == null)
{
return(0);
}
closedList.enqueue(currentNode); //add currentNode to the closedList
if (isBackwards == true)
{
if (currentNode.getPolygon().getHasAgent() == true)
{
finalSolutionStart = currentNode;
return(1);
}
}
else
{
if (currentNode.getPolygon().getHasGoal() == true) //checks to see if the currentNode has the goal inside its polygon
{
finalSolutionStart = currentNode;
return(1);
}
}
if (secondSearchClosedList.isNodeOnList(currentNode.getPolygon()) == true)
{
finalSolutionStart = currentNode;
return(2);
}
for (int count = 0; count < currentNode.getPolygon().getNeighborsHeld(); count++) //adds all the neighbors that are not on the closed list to the open list
{
if (closedList.isNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]) == false)
{
gCost = (currentNode.getPolygon().getCenterVector() - polygonArray[currentNode.getPolygon().getNeighborAt(count)].getCenterVector()).magnitude + currentNode.getGFromStartingNode();
if (openList.isNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]) == false)
{
openList.addNode(polygonArray[currentNode.getPolygon().getNeighborAt(count)], currentNode, gCost);
queueSize++;
}
else if (openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]).compareToG(gCost) > 0f) //updates the a Nodes information if the new GCost (cost from start to node) is less then what was previously in it
{
openList.updateNode(openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]), currentNode, gCost);
queueSize++;
}
}
}
if (openList.getSize() > maxQueueSize)
{
maxQueueSize = openList.getSize();
}
nodesExpandedCount++;
}
return(0);
}
/* deleteNodeOfId deletes the node from the list that is holding a polygon with a certain id
* Parameter: (int) idToDelete is the id of the polygon being held by the node to be deleted
* Return: none
*/
public void deleteNodeOfId(int idToDelete)
{
AIAgentAStarSearchNode tempFront = frontOfList;
if (frontOfList != null)
{
if (doesIDMatch(frontOfList.getPolygon().getID(), idToDelete) == true)
{
frontOfList = frontOfList.getNextNode();
numberOfNodesHeld--;
return;
}
AIAgentAStarSearchNode tempback = tempFront.getNextNode();
while ((tempback.getNextNode() != null) && (doesIDMatch(tempback.getPolygon().getID(), idToDelete) == false))
{
tempFront = tempback;
tempback = tempFront.getNextNode();
}
if (doesIDMatch(tempback.getPolygon().getID(), idToDelete) == true)
{
tempFront.setNextNode(tempback.getNextNode());
}
}
numberOfNodesHeld--;
}
/* updateNode updates the parent of a node and the gFromStartingNodeCost of a node
* Parameter: (AIAgentAStarSearchNode) nodeToUpdate is the node to be updated
* (AIAgentAStarSearchNode) newParentNode is the new parent of the node to be updated
* (float) newCost is the new gFromStartingNodeCost of the node to be updated
* Return: none
*/
public void updateNode(AIAgentAStarSearchNode nodeToUpdate, AIAgentAStarSearchNode newParentNode, float newCost)
{
AIPolygon temp = nodeToUpdate.getPolygon();
deleteNodeOfId(nodeToUpdate.getPolygon().getID());
addNode(temp, newParentNode, newCost);
}
/* enqueue adds a node to the front of this list
* Parameter: (AIAgentAStarSearchNode) nodeToAdd is the node to be added to the front of the list
* Return: none
*/
public void enqueue(AIAgentAStarSearchNode nodeToAdd)
{
AIAgentAStarSearchNode newNode = new AIAgentAStarSearchNode(nodeToAdd.getPolygon(), nodeToAdd.getParentNode(), nodeToAdd.getGFromStartingNode(), goalPosition);
newNode.setNextNode(frontOfList);
frontOfList = newNode;
}
/* addNode adds a node to this list in order according to its fTotalCost value
* Parameter: (AIPolygon) polygonToAdd is the polygon that will be held by the node
* (AIAgentAStarSearchNode) parentNodeToAdd is the parent of the node to be added
* (float) gCostToAdd is the getGFromStartingNode value to be stored in the node to be added
* Return: none
*/
public void addNode(AIPolygon polygonToAdd, AIAgentAStarSearchNode parentNodeToAdd, float gCostToAdd)
{
AIAgentAStarSearchNode newSearchNode = new AIAgentAStarSearchNode(polygonToAdd, parentNodeToAdd, gCostToAdd, goalPosition);
if (frontOfList == null)
{
newSearchNode.setNextNode(frontOfList);
frontOfList = newSearchNode;
numberOfNodesHeld++;
return;
}
AIAgentAStarSearchNode tempFront = frontOfList;
if (tempFront.compareTo(newSearchNode.getTotalCost()) > 0f)
{
newSearchNode.setNextNode(tempFront);
frontOfList = newSearchNode;
numberOfNodesHeld++;
return;
}
AIAgentAStarSearchNode tempBack = tempFront.getNextNode();
while (tempBack != null && tempBack.compareTo(newSearchNode.getTotalCost()) < 0f)
{
tempFront = tempBack;
tempBack = tempFront.getNextNode();
}
newSearchNode.setNextNode(tempFront.getNextNode());
tempFront.setNextNode(newSearchNode);
numberOfNodesHeld++;
}
//used for debugging
void printSolutionRecusively(AIAgentAStarSearchNode currentNode)
{
if (currentNode != null)
{
printSolutionRecusively(currentNode.getParentNode());
polygonFinalCount++;
}
}
//used for debugging
void printSolutionRecusively(AIAgentAStarSearchNode currentNode)
{
if (currentNode != null)
{
printSolutionRecusively(currentNode.getParentNode());
Debug.Log(Time.realtimeSinceStartup + " id = " + currentNode.getPolygon().getID());
polygonFinalCount++;
}
}
/* AIAgentAStarSearchNode method is a constructor for this class.
* Parmeters: (AIPolygon) polygonToAdd is the polygon held by this node
* (AIAgentAStarSearchNode) parentToAdd is the parent of this node
* (float) gCostToAdd is the gFromStartingNode value for this node
* (Vector3) goalPositionToAdd is the goalPosition for this instance
*/
public AIAgentAStarSearchNode(AIPolygon polygonToAdd, AIAgentAStarSearchNode parentToAdd, float gCostToAdd, Vector3 goalPositionToAdd)
{
polygonBeingHeld = polygonToAdd;
parentNode = parentToAdd;
gFromStartingNode = gCostToAdd;
nextNode = null;
goalPosition = new Vector3(goalPositionToAdd.x, goalPositionToAdd.y, goalPositionToAdd.z);
calculateCost(gFromStartingNode, goalPosition);
}
/*
* swap method will swap two nodes in the heap according to the indices passed in
* Parameter: (int)first is the index of the first node to swap
* (int)second is the index of the second node to swap
* Return: none
*/
void swap(int first, int second)
{
AIAgentAStarSearchNode tempNode = heap [first];
heap [first] = heap [second];
indicesArray [heap [first].getPolygon().getID()] = first;
heap [second] = tempNode;
indicesArray [heap [second].getPolygon().getID()] = second;
}
/*
* addFinalsolutionPolygonsPostOrder method is a recusive method that will look at the parent of each node passed in until
* the node passed in is null, then it will add each Node's polygon to the finalSolutionArray in pre order
* from start until end
* Parameter: (AIAgentAStarSearchNode)currentNode is the node that needs to be checked and then have its parent passed
* (ref int)counter is the current count of polygons in the FinalSolution array used to access the next index
* Return: none
*/
void addFinalSolutionPolygonsPreOrder(AIAgentAStarSearchNode currentNode, ref int counter)
{
if (currentNode != null)
{
finalSolutionArray [counter] = currentNode.getPolygon();
counter++;
addFinalSolutionPolygonsPreOrder(currentNode.getParentNode(), ref counter);
}
}
/* addNode adds a node to this list in order according to its fTotalCost value
* Parameter: (AIPolygon) polygonToAdd is the polygon that will be held by the node
* (AIAgentAStarSearchNode) parentNodeToAdd is the parent of the node to be added
* (float) gCostToAdd is the getGFromStartingNode value to be stored in the node to be added
* Return: none
*/
public void addNode(AIPolygon polygonToAdd, AIAgentAStarSearchNode parentNodeToAdd, float gCostToAdd)
{
AIAgentAStarSearchNode newSearchNode = new AIAgentAStarSearchNode(polygonToAdd, parentNodeToAdd, gCostToAdd, goalPosition);
inList [polygonToAdd.getID()] = true;
indicesArray [polygonToAdd.getID()] = numberOfNodesHeld;
heap [numberOfNodesHeld] = newSearchNode;
shiftUp(numberOfNodesHeld);
numberOfNodesHeld++;
}
/*
* getFinalPathStartingWithNodes method will return a final path array that starts the the node
* containing the polygon being sent in
* Parameter: (AIPolygon) polygonToStartWith is the polygon that is in the node the caller wants the
* final path array to start with
* Return: (AIPolygon[])
* the final path array starting with the node containing the polygon being passed in
*/
public AIPolygon[] getFinalPathStartingWithNode(AIPolygon polygonToStartWith)
{
AIAgentAStarSearchNode tempNode = closedList[polygonToStartWith.getID()];
if (tempNode == null)
{
return(null);
}
finalSolutionStart = tempNode;
return(getFinalPath());
}
/* popNode removes and returns the node at the front of the list (node with lowest fToatlCost value)
* Parameter: none
* Return: (AIAgentAStarSearchNode)
* node that was removed from the front of this list
*/
public AIAgentAStarSearchNode popNode()
{
if (frontOfList == null)
{
return(null);
}
AIAgentAStarSearchNode tempNode = frontOfList;
frontOfList = tempNode.getNextNode();
numberOfNodesHeld--;
return(tempNode);
}
/*
* addFinalsolutionPolygons method is a recusive method that will look at the parent of each node passed in until
* the node passed in is null, then it will add each Node's polygon to the finalSolutionArray in order
* from start until end
* Parameter: (AIAgentAStarSearchNode)currentNode is the node that needs to be checked and then have its parent passed
* (ref int)counter is the current count of polygons in the FinalSolution array used to access the next index
* Return: none
*/
void addFinalSolutionPolygons(AIAgentAStarSearchNode currentNode, ref int counter)
{
if (currentNode != null)
{
addFinalSolutionPolygons(currentNode.getParentNode(), ref counter);
finalSolutionArray [counter] = currentNode.getPolygon();
if (counter != 0)
{
finalPathCost += (finalSolutionArray[counter].getCenterVector() - finalSolutionArray[counter - 1].getCenterVector()).magnitude;
}
counter++;
}
}
/*
* AStarSearch method will preform an A* algorithm for searching a space to find a goal. It does this by taking the best
* polygon for the search off the openList, adding its neighbors to the openlist, placing the node on the closed list
* then repeating until it found a polygon that has the goal gameObject inside it
* Parameters: none
* Return: none
*/
void AStarSearch()
{
maxQueueSize = 1;
AIAgentAStarSearchNode currentNode;
float gCost = 0f;
bool[] closedList2 = new bool[polygonArray.Length]; //use to replace the close list to help reduce linear searches
for (int count = 0; count < polygonArray.Length; count++)
{
closedList2 [count] = false;
}
while (openList.isEmpty() == false) //goes until nothing is left on the open list meaning a path could not be found
{
currentNode = openList.popNode(); //take the first(Best) polygon off the openList
nodesVisited++;
if (currentNode == null)
{
return;
}
closedList2[currentNode.getPolygon().getID()] = true; //adds node to the close list
if (currentNode.getPolygon().getHasGoal() == true) //checks to see if the currentNode has the goal inside its polygon
{
finalSolutionStart = currentNode;
return;
}
for (int count = 0; count < currentNode.getPolygon().getNeighborsHeld(); count++) //adds all the neighbors that are not on the closed list to the open list
{
if (closedList2[currentNode.getPolygon().getNeighborAt(count)] == false) //checks to see if a node is logically should be in the closed list
{
gCost = (currentNode.getPolygon().getCenterVector() - polygonArray[currentNode.getPolygon().getNeighborAt(count)].getCenterVector()).magnitude + currentNode.getGFromStartingNode();
if (openList.isNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]) == false)
{
openList.addNode(polygonArray[currentNode.getPolygon().getNeighborAt(count)], currentNode, gCost);
}
else if (openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]).compareToG(gCost) > 0f) //updates the a Nodes information if the new GCost (cost from start to node) is less then what was previously in it
{
openList.updateNode(openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]), currentNode, gCost);
}
}
}
if (openList.getSize() > maxQueueSize)
{
maxQueueSize = openList.getSize();
}
}
}
/* popNode removes and returns the node at the front of the list (node with lowest fToatlCost value)
* Parameter: none
* Return: (AIAgentAStarSearchNode)
* node that was removed from the front of this list
*/
public AIAgentAStarSearchNode popNode()
{
if (numberOfNodesHeld == 0)
{
return(null);
}
AIAgentAStarSearchNode tempNode = heap [0];
indicesArray [tempNode.getPolygon().getID()] = -1;
numberOfNodesHeld--;
heap [0] = heap [numberOfNodesHeld];
indicesArray [heap [0].getPolygon().getID()] = 0;
if (numberOfNodesHeld > 0)
{
shiftDown(0);
}
return(tempNode);
}
/*
* AStarSearch method will preform an A* algorithm for searching a space to find a goal. It does this by taking the best
* polygon for the search off the openList, adding its neighbors to the openlist, placing the node on the closed list
* then repeating until it found a polygon that has the goal gameObject inside it
* Parameters: none
* Return: none
*/
void AStarSearch()
{
maxQueueSize = 1;
AIAgentAStarSearchNode currentNode;
float gCost = 0f;
while (openList.isEmpty() == false) //goes until nothing is left on the open list meaning a path could not be found
{
currentNode = openList.popNode(); //take the first(Best) polygon off the openList
nodesVisited++;
if (currentNode == null)
{
return;
}
closedList2[currentNode.getPolygon().getID()] = true;
if (currentNode.getPolygon().getHasGoal() == true) //checks to see if the currentNode has the goal inside its polygon
{
finalSolutionStart = currentNode;
return;
}
for (int count = 0; count < currentNode.getPolygon().getNeighborsHeld(); count++) //adds all the neighbors that are not on the closed list to the open list
{
if (closedList2[currentNode.getPolygon().getNeighborAt(count)] == false)
{
gCost = (currentNode.getPolygon().getCenterVector() - polygonArray[currentNode.getPolygon().getNeighborAt(count)].getCenterVector()).magnitude + currentNode.getGFromStartingNode();
if (openList.isNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]) == false)
{
openList.addNode(polygonArray[currentNode.getPolygon().getNeighborAt(count)], currentNode, gCost);
}
else if (openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]).compareToG(gCost) > 0f) //updates the a Nodes information if the new GCost (cost from start to node) is less then what was previously in it
{
//Debug.Log(Time.realtimeSinceStartup + " updating polygon id = " + openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]).getPolygon().getID());
openList.updateNode(openList.getNodeOnList(polygonArray[currentNode.getPolygon().getNeighborAt(count)]), currentNode, gCost);
}
}
}
if (openList.getSize() > maxQueueSize)
{
maxQueueSize = openList.getSize();
}
}
}
/* setParentNode sets the parent node for this instance
* Parameter: (AIAgentAStarSearchNode) newParentNode is the node to be set as this instance's parent node
* Return: none
*/
public void setParentNode(AIAgentAStarSearchNode newParentNode)
{
parentNode = newParentNode;
}
/* setNextNode sets a new nextNode value for the current node
* Parameter: (AIAgentAStarSearchNode) newNextNode is the node to be set as this instances nextNode
* Return: none
*/
public void setNextNode(AIAgentAStarSearchNode newNextNode)
{
nextNode = newNextNode;
}
Vector3 goalPosition; // stores the goal position
/* AIAgentAStarSearchList method is a constructor for this class and will set up the variables to the initial values
* Parameter: (Vector3) goalPositionToAdd
*/
public AIAgentAStarSearchList(Vector3 goalPositionToAdd)
{
frontOfList = null;
numberOfNodesHeld = 0;
goalPosition = goalPositionToAdd;
}