public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
this.Open.Initialize();
this.Closed.Initialize();
NodeRecord StartNode = NodeRecordArray.GetNodeRecord(startNode);
StartNode.StartNodeOutConnectionIndex = -1;
Open.AddToOpen(StartNode);
int OpenSize = Open.All().Count;
while (OpenSize > 0)
{
NodeRecord currentNode = Open.GetBestAndRemove();
Open.RemoveFromOpen(currentNode);
Closed.AddToClosed(currentNode);
if (currentNode.StartNodeOutConnectionIndex != -1)
{
nodeGoalBounds.connectionBounds[currentNode.StartNodeOutConnectionIndex].UpdateBounds(currentNode.node.Position);
}
//Initialize start node edge colors:
var outConnections = currentNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
this.ProcessChildNode(currentNode, currentNode.node.EdgeOut(i), i);
}
OpenSize = Open.All().Count;
}
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.node = startNode;
startNodeRecord.gValue = 0f;
this.Open.AddToOpen(startNodeRecord);
while (this.Open.CountOpen() > 0)
{
NodeRecord Node = this.Open.GetBestAndRemove();
this.Closed.AddToClosed(Node);
if (Node.id != -1 && nodeGoalBounds.connectionBounds.Length > Node.id)
{
nodeGoalBounds.connectionBounds[Node.id].UpdateBounds(Node.node.LocalPosition);
}
for (int i = 0; i < Node.node.OutEdgeCount; i++)
{
ProcessChildNode(Node, Node.node.EdgeOut(i), i);
}
}
this.Open.Initialize();
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
this.Open.Initialize();
this.Closed.Initialize();
NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.gValue = 0;
startNodeRecord.fValue = 0;
startNodeRecord.parent = null;
startNodeRecord.StartNodeOutConnectionIndex = 0;
Open.AddToOpen(startNodeRecord);
while (true)
{
if (this.Open.CountOpen() == 0)
{
return;
}
NodeRecord currNode = this.Open.GetBestAndRemove();
this.Closed.AddToClosed(currNode);
nodeGoalBounds.connectionBounds[currNode.StartNodeOutConnectionIndex].UpdateBounds(currNode.node.Position);
int nOutConnections = currNode.node.OutEdgeCount;
for (int i = 0; i < nOutConnections; i++)
{
this.ProcessChildNode(currNode, currNode.node.EdgeOut(i), i);
}
}
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
this.StartNode = startNode;
this.NodeGoalBounds = nodeGoalBounds;
// we fill the initial childNodes with start edge indexes,
// and add them to the open list. the start node is added to closed list
this.Initialize(startNode);
var closed = this.Closed.All();
var openCount = this.Open.CountOpen();
while (openCount > 0)
{
var currentNode = this.Open.GetBestAndRemove();
this.Closed.AddToClosed(currentNode);
for (var i = 0; i < currentNode.node.OutEdgeCount; i++)
{
ProcessChildNode(currentNode, currentNode.node.EdgeOut(i), currentNode.StartNodeOutConnectionIndex);
}
openCount = this.Open.CountOpen();
}
}
protected override void ProcessChildNode(NodeRecord parentNode, NavigationGraphEdge connectionEdge, int edgeIndex)
{
NodeGoalBounds nodeBounds = this.GoalBoundingTable.table [parentNode.node.NodeIndex];
this.TotalEdges++;
if (nodeBounds == null)
{
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
}
else
{
if (nodeBounds.connectionBounds.Length - 1 < edgeIndex)
{
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
return;
}
else if (nodeBounds.connectionBounds [edgeIndex].PositionInsideBounds(this.GoalPosition))
{
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
return;
}
this.DiscardedEdges++;
}
}
protected override void ProcessChildNode(NodeRecord parentNode, NavigationGraphEdge connectionEdge, int edgeIndex)
{
//TODO: Implement this method for the GoalBoundingPathfinding to Work. If you implemented the NodeArrayAStar properly, you wont need to change the search method.
//Fetching index of GoalBoundingTable
NodeRecord childNodeRecord = NodeRecordArray.GetNodeRecord(connectionEdge.ToNode);
int index = childNodeRecord.node.NodeIndex;
NodeGoalBounds nodeBounds = GoalBoundingTable.table[index];
if (nodeBounds == null) //Special check for some nodes that have null nodeBounds??? is NodeIndex correct or is the table malformed?
{
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
}
//TODO: needs fix
/* Check if the box of parent node in the direction of the childnode has goal within. Not working due to arrayIndexExceptions:
*
* DataStructures.GoalBounding.Bounds b = nodeBounds.connectionBounds[edgeIndex];
* if (!b.PositionInsideBounds(GoalPosition))
* {
* this.DiscardedEdges++;
* return;
* }
*/
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.StartNodeOutConnectionIndex = -1;
startNodeRecord.fValue = 0;
startNodeRecord.parent = null;
for (int i = 0; i < startNode.OutEdgeCount; i++)
{
NodeRecord nodeChildRecord = this.NodeRecordArray.GetNodeRecord(startNode.EdgeOut(i).ToNode);
nodeChildRecord.parent = startNodeRecord;
nodeChildRecord.StartNodeOutConnectionIndex = i;
nodeChildRecord.fValue = (startNodeRecord.node.Position - nodeChildRecord.node.Position).magnitude;
this.Open.AddToOpen(nodeChildRecord);
}
this.Closed.AddToClosed(startNodeRecord);
while (this.Open.CountOpen() > 0)
{
var bestRecord = this.Open.GetBestAndRemove();
for (int i = 0; i < bestRecord.node.OutEdgeCount; i++)
{
this.ProcessChildNode(bestRecord, bestRecord.node.EdgeOut(i), i);
}
nodeGoalBounds.connectionBounds [bestRecord.StartNodeOutConnectionIndex].UpdateBounds(bestRecord.node.Position);
this.Closed.AddToClosed(bestRecord);
}
}
public DijkstraSearchThread(NavigationGraphNode startNode, NodeGoalBounds bounds, GoalBoundsDijkstraMapFlooding dijkstra, GoalBoundingTable goalBoundingTable, int index)
{
this.startNode = startNode;
this.bounds = bounds;
this.dijkstra = dijkstra;
this.goalBoundingTable = goalBoundingTable;
this.index = index;
}
private static void CalculateGoalBounds()
{
//get the NavMeshGraph from the current scene
NavMeshPathGraph navMesh = GameObject.Find("Navigation Mesh").GetComponent <NavMeshRig>().NavMesh.Graph;
//this is needed because RAIN AI does some initialization the first time the QuantizeToNode method is called
//if this method is not called, the connections in the navigationgraph are not properly initialized
navMesh.QuantizeToNode(new Vector3(0, 0, 0), 1.0f);
var dijkstra = new GoalBoundsDijkstraMapFlooding(navMesh);
GoalBoundingTable goalBoundingTable = new GoalBoundingTable();
var nodes = GetNodesHack(navMesh);
goalBoundingTable.table = new NodeGoalBounds[nodes.Count];
NodeGoalBounds auxGoalBounds;
//calculate goal bounds for each edge
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] is NavMeshEdge)
{
//initialize the GoalBounds structure for the edge
auxGoalBounds = new NodeGoalBounds
{
connectionBounds = new Assets.Scripts.IAJ.Unity.Pathfinding.DataStructures.GoalBounding.Bounds[nodes[i].OutEdgeCount]
};
for (int j = 0; j < nodes[i].OutEdgeCount; j++)
{
auxGoalBounds.connectionBounds[j] = new Assets.Scripts.IAJ.Unity.Pathfinding.DataStructures.GoalBounding.Bounds();
auxGoalBounds.connectionBounds[j].InitializeBounds(nodes[i].LocalPosition);
}
if (i % 10 == 0)
{
float percentage = (float)i / (float)nodes.Count;
EditorUtility.DisplayProgressBar("GoalBounding precomputation progress", "Calculating goal bounds for each edge", percentage);
}
//run a Dijkstra mapflooding for each node
dijkstra.Search(nodes[i], auxGoalBounds);
goalBoundingTable.table[i] = auxGoalBounds;
}
}
//saving the table to a binary file
EditorUtility.DisplayProgressBar("GoalBounding precomputation progress", "Saving GoalBoundsTable to an Asset file", 100);
goalBoundingTable.Save(Application.dataPath + "/Resources/", "GoalBoundingTable.dat");
//saving the assets, this takes forever using Unity's serialization mechanism
EditorUtility.ClearProgressBar();
}
protected override void ProcessChildNode(NodeRecord parentNode, NavigationGraphEdge connectionEdge, int edgeIndex)
{
NodeGoalBounds goalBounds = this.GoalBoundingTable.table[parentNode.node.NodeIndex];
if (goalBounds != null && goalBounds.connectionBounds.Length > edgeIndex)
{
if (!goalBounds.connectionBounds[edgeIndex].PositionInsideBounds(this.GoalPosition))
{
return;
}
}
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
this.Open.Initialize();
this.Closed.Initialize();
//Initialize starting node for Dijkstra
NodeRecord StartNode = NodeRecordArray.GetNodeRecord(startNode);
StartNode.gValue = 0;
StartNode.StartNodeOutConnectionIndex = -1; // -1 corresponds to not having a "color". Valid indices will start at 0
Open.AddToOpen(StartNode);
//Dijkstra
while (Open.CountOpen() > 0)
{
NodeRecord currentNode = Open.GetBestAndRemove();
Open.RemoveFromOpen(currentNode);
Closed.AddToClosed(currentNode);
//We don't fill out the starting position as it is colorless
if (currentNode.StartNodeOutConnectionIndex != -1)
{
var outFillConnections = currentNode.node.OutEdgeCount;
Vector3 edgePosition;
//Update the bounding box with all positions of EdgeOuts of the current Node
for (int i = 0; i < outFillConnections; i++)
{
edgePosition = currentNode.node.EdgeOut(i).ToNode.Position;
nodeGoalBounds.connectionBounds[currentNode.StartNodeOutConnectionIndex].UpdateBounds(edgePosition); //update only the bound corresponding to the ConnectionIndex
}
}
//Process Child Nodes
var outConnections = currentNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
this.ProcessChildNode(currentNode, currentNode.node.EdgeOut(i), i);
}
}
}
//cria rectangulos
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
// mete os vizinhos do no inicial. Inicializacao da lista
var outConnections = startNode.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
NavigationGraphEdge edge = startNode.EdgeOut(i);
var childNode = edge.ToNode;
var childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode);
//adicionar ao open
NodeRecordArray.AddToOpen(childNodeRecord);
//transformar em vector3 para inicializar cada rectangulo
childNodeRecord.StartNodeOutConnectionIndex = i;
}
//giro: var startTime = Time.realtimeSinceStartup;
//enquanto houver nos no conj open
while (this.Open.CountOpen() > 0)
{
NodeRecord bestNode = this.Open.GetBestAndRemove();
//aumentar o rectangulo
nodeGoalBounds.connectionBounds[bestNode.StartNodeOutConnectionIndex].UpdateBounds(bestNode.node.LocalPosition); //isto e a cor do rectangulo. falta updateBounds
this.Closed.AddToClosed(bestNode);
//para ver as ligacoes do no que acabamos de ver
var outConnections2 = bestNode.node.OutEdgeCount;
for (int j = 0; j < outConnections2; j++)
{
this.ProcessChildNode(bestNode, bestNode.node.EdgeOut(j), bestNode.StartNodeOutConnectionIndex);
}
// giro: this.MaxOpenNodes = Mathf.Max(this.Open.CountOpen(), this.MaxOpenNodes);
}
}
protected override void ProcessChildNode(NodeRecord parentNode, NavigationGraphEdge connectionEdge, int edgeIndex)
{
//TODO: Implement this method for the GoalBoundingPathfinding to Work. If you implemented the NodeArrayAStar properly, you wont need to change the search method.
//Fetching index of GoalBoundingTable
int index = parentNode.node.NodeIndex;
NodeGoalBounds nodeBounds = GoalBoundingTable.table[index];
if (nodeBounds == null) //Special check for some nodes that have null nodeBounds.
{
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
}
if (nodeBounds != null && nodeBounds.connectionBounds.Length > edgeIndex) // Special check for when bounds are not available for the node even if it exists.
{
//Obtain the parent bound corresponding to the edgeIndex of the child.
DataStructures.GoalBounding.Bounds b = nodeBounds.connectionBounds[edgeIndex];
if (!b.PositionInsideBounds(GoalPosition))
{
this.DiscardedEdges++;
return;
}
}
base.ProcessChildNode(parentNode, connectionEdge, edgeIndex);
}
protected override void ProcessChildNode(NodeRecord parentNode, NavigationGraphEdge connectionEdge, int connectionIndex)
{
TotalEdges++;
NodeGoalBounds goalBound = GoalBoundingTable.table[parentNode.node.NodeIndex];
if (goalBound != null && connectionIndex < goalBound.connectionBounds.Length)
{
DataStructures.GoalBounding.Bounds bound = goalBound.connectionBounds[connectionIndex];
if (bound.PositionInsideBounds(GoalNode.Position))
{
base.ProcessChildNode(parentNode, connectionEdge, connectionIndex);
return;
}
}
else
{
base.ProcessChildNode(parentNode, connectionEdge, connectionIndex);
return;
}
DiscardedEdges++;
return;
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeIndex = startNodeRecord.node.NodeIndex;
startNodeRecord.startNodeIndex = startNodeIndex;
bool first = true;
Open.AddToOpen(startNodeRecord);
while (this.Open.CountOpen() > 0)
{
bestNode = this.Open.GetBestAndRemove();
if (!first)
{
nodeGoalBounds.connectionBounds[bestNode.StartNodeOutConnectionIndex].UpdateBounds(bestNode.node.Position);
}
this.Closed.AddToClosed(bestNode);
for (int i = 0; i < bestNode.node.OutEdgeCount; i++)
{
if (first)
{
this.ProcessChildNode(bestNode, bestNode.node.EdgeOut(i), i, nodeGoalBounds);
}
else
{
this.ProcessChildNode(bestNode, bestNode.node.EdgeOut(i), bestNode.StartNodeOutConnectionIndex, nodeGoalBounds);
}
}
first = false;
}
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
this.Open.Initialize();
this.Closed.Initialize();
StartNode = startNode;
var startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.gValue = 0;
startNodeRecord.hValue = 0;
startNodeRecord.fValue = F(startNodeRecord);
Closed.AddToClosed(startNodeRecord);
var outConnectionsStart = startNodeRecord.node.OutEdgeCount;
for (int i = 0; i < outConnectionsStart; i++)
{
ProcessChildNode(startNodeRecord, startNodeRecord.node.EdgeOut(i), i);
NavigationGraphNode childNode = startNodeRecord.node.EdgeOut(i).ToNode;
var childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode);
nodeGoalBounds.connectionBounds[i].InitializeBounds(childNodeRecord.node.LocalPosition);
}
while (Open.CountOpen() > 0)
{
var bestNode = Open.GetBestAndRemove();
this.Closed.AddToClosed(bestNode);
//UnityEngine.Debug.Log("Parent index: " + bestNode.StartNodeOutConnectionIndex);
nodeGoalBounds.connectionBounds[bestNode.StartNodeOutConnectionIndex].UpdateBounds(bestNode.node.LocalPosition);
var outConnections = bestNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
ProcessChildNode(bestNode, bestNode.node.EdgeOut(i), bestNode.StartNodeOutConnectionIndex);
}
}
}
protected void ProcessChildNode(NodeRecord bestNode, NavigationGraphEdge connectionEdge, int connectionIndex, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement this method that processes a child node. Then you can use it in the Search method above.
var childNode = connectionEdge.ToNode;
NodeRecord childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode);
if (childNodeRecord.startNodeIndex != startNodeIndex)
{
childNodeRecord.status = NodeStatus.Unvisited;
}
if (childNodeRecord == null)
{
//this piece of code is used just because of the special start nodes and goal nodes added to the RAIN Navigation graph when a new search is performed.
//Since these special goals were not in the original navigation graph, they will not be stored in the NodeRecordArray and we will have to add them
//to a special structure
//it's ok if you don't understand this, this is a hack and not part of the NodeArrayA* algorithm, just do NOT CHANGE THIS, or your algorithm will not work
childNodeRecord = new NodeRecord
{
node = childNode,
parent = bestNode,
status = NodeStatus.Unvisited
};
this.NodeRecordArray.AddSpecialCaseNode(childNodeRecord);
}
if (childNodeRecord.status == NodeStatus.Closed)
{
return;
}
float g = bestNode.gValue + connectionEdge.Cost;
float f = g;
if (childNodeRecord.status == NodeStatus.Unvisited)
{
ChangeNodeValues(childNodeRecord, 0, g, f, bestNode, connectionIndex);
NodeRecordArray.AddToOpen(childNodeRecord);
return;
}
if (childNodeRecord.status == NodeStatus.Open && childNodeRecord.fValue > f)
{
ChangeNodeValues(childNodeRecord, 0, g, f, bestNode, connectionIndex);
this.NodeRecordArray.Replace(childNodeRecord, childNodeRecord);
return;
}
}
private static void CalculateGoalBounds()
{
Debug.Log("Creating goalbounds table");
DateTime startTime = DateTime.Now;
//get the NavMeshGraph from the current scene
NavMeshPathGraph navMesh = GameObject.Find("Navigation Mesh").GetComponent <NavMeshRig>().NavMesh.Graph;
//this is needed because RAIN AI does some initialization the first time the QuantizeToNode method is called
//if this method is not called, the connections in the navigationgraph are not properly initialized
navMesh.QuantizeToNode(new Vector3(0, 0, 0), 1.0f);
GoalBoundingTable goalBoundingTable = new GoalBoundingTable();
var nodes = GetNodesHack(navMesh);
goalBoundingTable.table = new NodeGoalBounds[nodes.Count];
var dijkstra = new GoalBoundsDijkstraMapFlooding(nodes);
NodeGoalBounds auxGoalBounds;
//calculate goal bounds for each edge
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] is NavMeshEdge)
{
//initialize the GoalBounds structure for the edge
auxGoalBounds = new NodeGoalBounds();
auxGoalBounds.connectionBounds = new Assets.Scripts.IAJ.Unity.Pathfinding.DataStructures.GoalBounding.Bounds[nodes[i].OutEdgeCount];
for (int j = 0; j < nodes[i].OutEdgeCount; j++)
{
auxGoalBounds.connectionBounds[j] = new Assets.Scripts.IAJ.Unity.Pathfinding.DataStructures.GoalBounding.Bounds();
auxGoalBounds.connectionBounds[j].InitializeBounds(nodes[i].Position);
}
if (i % 10 == 0)
{
float percentage = (float)i / (float)nodes.Count;
EditorUtility.DisplayProgressBar("GoalBounding precomputation progress", "Calculating goal bounds for each edge", percentage);
}
//run a Dijkstra mapflooding for each node
dijkstra.Search(nodes[i], auxGoalBounds);
goalBoundingTable.table[i] = auxGoalBounds;
//edgeIndex++;
}
}
BinaryFormatter bf = new BinaryFormatter();
if (File.Exists("Assets/GoalBoundingData/GoalBounding.dat"))
{
File.Delete("Assets/GoalBoundingData/GoalBounding.dat");
}
FileStream file = File.Create("Assets/GoalBoundingData/GoalBounding.dat");
bf.Serialize(file, goalBoundingTable);
file.Close();
EditorUtility.ClearProgressBar();
TimeSpan time = DateTime.Now - startTime;
Debug.Log("Duration creating goalboundtable : " + time.ToString());
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
}
