private void ApplyDijkstra(NavigationGraphNode node) { Open.Initialize(); Closed.Initialize(); var currentNode = node; GoalBoundingRecord currentNodeRecord = new GoalBoundingRecord(null, currentNode, null); Closed.AddToClosed(currentNodeRecord); int outConnections = currentNode.OutEdgeCount; if (outConnections <= 0) { return; } int numOfBB = outConnections > maxNumberOfBoundingBoxes ? maxNumberOfBoundingBoxes : outConnections; boundingBoxes[currentNode.NodeIndex] = new List<BoundingBox>(new BoundingBox[numOfBB]); for (int j = 0; j < numOfBB; j++) { boundingBoxes[currentNode.NodeIndex][j] = new BoundingBox(); NavigationGraphNode neighborNode = currentNode.EdgeOut(j).ToNode; Open.AddToOpen(new GoalBoundingRecord(currentNodeRecord, neighborNode, boundingBoxes[currentNode.NodeIndex][j])); } while (Open.CountOpen() > 0) { processedNodes++; var currentRecord = Open.GetBestAndRemove(); Closed.AddToClosed(currentRecord); //needs a function to get adjacent nodes // NavMeshNode outConnections = currentRecord.Location.OutEdgeCount; for (int i = 0; i < outConnections; i++) { DummyRecord.Location = currentRecord.Location.EdgeOut(i).ToNode; DummyRecord.Distance = GoalBoundingRecord.CalculateDistance(currentRecord, DummyRecord); GoalBoundingRecord neighborRecord = null; neighborRecord = Closed.SearchInClosed(DummyRecord); if (neighborRecord != null) { //if (neighborRecord.Distance >= DummyRecord.Distance) // Closed.RemoveFromClosed(neighborRecord); //else continue; } else { neighborRecord = Open.SearchInOpen(DummyRecord); if (neighborRecord != null) { if (neighborRecord.Distance > DummyRecord.Distance) { neighborRecord.UpdatePrevious(currentRecord); } continue; } else { neighborRecord = new GoalBoundingRecord(); neighborRecord.Location = DummyRecord.Location; } } neighborRecord.UpdatePrevious(currentRecord); Open.AddToOpen(neighborRecord); } } }
private void ApplayDijkstraAll() { if (navMesh != null) { nodes = GetNodesHack(navMesh); if (useNodeRecordArray) { Debug.Log("UsingNodeRecordArray"); recordArray = new NodeRecordArray<NavigationGraphNode, GoalBoundingRecord>(nodes); Open = recordArray; Closed = recordArray; } else { Debug.Log("NotUsingNodeRecordArray"); Open = new LeftPriorityList<GoalBoundingRecord>(); Closed = new ClosedDictionary<NavigationGraphNode, GoalBoundingRecord>(); } DummyRecord = new GoalBoundingRecord(); float runningTime = Time.realtimeSinceStartup; boundingBoxes = new List<List<BoundingBox>>(new List<BoundingBox>[nodes.Count]); for (int i = 0; i < nodes.Count; i++) { nodes[i].NodeIndex = i; } for (int k = 0; k < nodes.Count; k++) { if (useNodeRecordArray) { ApplyDijkstraNodeArray(nodes[k]); } else { ApplyDijkstra(nodes[k]); } ICollection<GoalBoundingRecord> closed = Closed.All(); foreach (GoalBoundingRecord record in closed) { if (record.OriginBoundingBox != null) { record.OriginBoundingBox.Update(record.Location.Position); } } if (boundingBoxes[k] != null) { foreach (BoundingBox boundingBox in boundingBoxes[k]) { boundingBox.Update(nodes[k].Position); } } PrintTimeFunc(k, Time.realtimeSinceStartup - runningTime); } //last print Debug.Log("Nodes: " + nodes.Count + " Processing time: " + (Time.realtimeSinceStartup - runningTime) + " s"); } }