public PathData GetPath( VertexScript startNode, VertexScript goalNode, TravelerProfile traveler = null, CostMethodType costMethod = CostMethodType.Steepness, // <- Default cost method int maxDepth = int.MaxValue) { PathData data = null; if (startNode == null || goalNode == null) { return(null); } else if (startNode == goalNode) { Debug.Log("ERROR: startNode == goalNode in GetPath."); return(null); } if (traveler == null) { traveler = TravelerProfileCatalog.GetProfile(TravelerProfileCatalog.TravelerType.Heart); // <- Setting defaults to heart } PriorityQueue <VertexWrapper> priorityQueue = new PriorityQueue <VertexWrapper> (); Dictionary <VertexScript, VertexWrapper> vertexDict = new Dictionary <VertexScript, VertexWrapper> (); Dictionary <EdgeScript, EdgeWrapper> edgeDict = new Dictionary <EdgeScript, EdgeWrapper> (); EdgeWrapper edgeWrapper; bool reachedGoal = false; VertexWrapper nodeWrapper = GetOrMakeVertexWrapper(startNode, vertexDict); nodeWrapper.LowestCostSoFar = 0; priorityQueue.Enqueue(nodeWrapper); while (!reachedGoal && priorityQueue.Count != 0) { VertexWrapper tempWrapper = priorityQueue.Dequeue(); if (tempWrapper != null) { nodeWrapper = tempWrapper; if (nodeWrapper.Vertex == goalNode) { reachedGoal = true; break; } if (nodeWrapper.Depth == maxDepth) { break; } VertexScript currentNode = nodeWrapper.Vertex; foreach (EdgeScript edge in currentNode.Edges) { if (edge == null) { continue; } VertexScript otherNode = edge.GetOtherVertex(currentNode); VertexWrapper otherNodeWrapper = GetOrMakeVertexWrapper(otherNode, vertexDict); if (otherNode != null) { // returns the edge cost // float cost = GetCost(nodeWrapper, otherNodeWrapper, edge, costMethod, traveler, startNode, goalNode); actor.setCurBPM(cost); // // // // // // // // // Debug.Assert(cost >= 0f, "ERROR: Dijkstra's Algorithm does not accept negative edge weights."); float costOfPathPlusThisEdge = nodeWrapper.LowestCostSoFar + cost; if (costOfPathPlusThisEdge < otherNodeWrapper.LowestCostSoFar) { edgeWrapper = GetOrMakeEdgeWrapper(edge, edgeDict); edgeWrapper.Cost = cost; otherNodeWrapper.LowestCostSoFar = costOfPathPlusThisEdge; otherNodeWrapper.LowestCostEdgeSoFar = edgeWrapper; otherNodeWrapper.Depth = nodeWrapper.Depth + 1; priorityQueue.Enqueue(otherNodeWrapper); } } } } } data = new PathData(startNode, goalNode, nodeWrapper.LowestCostSoFar); data.InsertPreviousStep(nodeWrapper.Vertex); // traverse the shortest path from the goalNode backwards to the startNode edgeWrapper = nodeWrapper.LowestCostEdgeSoFar; while (edgeWrapper != null) { nodeWrapper = GetOrMakeVertexWrapper(edgeWrapper.Edge.GetOtherVertex(nodeWrapper.Vertex), vertexDict); edgeWrapper = nodeWrapper.LowestCostEdgeSoFar; data.InsertPreviousStep(nodeWrapper.Vertex); } Debug.Assert(nodeWrapper.Vertex == startNode, "ERROR: shortest path did not terminate at startNode in MakeShortestPath."); priorityQueue.Clear(); vertexDict.Clear(); edgeDict.Clear(); return(data); }