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);
}
