public static void prepareForCalculation(CalculatePathRequest p_request, NavigationGraph p_navigationGraph, NavigationNode p_beginNode, NavigationNode p_endNode, PathCalculationParameters p_pathCalculationParameters) { p_request.NavigationGraph = p_navigationGraph; p_request.BeginNode = p_beginNode; p_request.EndNode = p_endNode; p_request.PathCalculationParameters = p_pathCalculationParameters; }
public static CalculatePathRequest alloc() { CalculatePathRequest l_instance = new CalculatePathRequest(); l_instance.NodesElligibleForNextCurrent = new List <NavigationNode>(); l_instance.ResultPath = NavigationPath.build(new List <NavigationNode>(), new Dictionary <NavigationNode, NavigationNodePathTraversalCalculations>()); return(l_instance); }
static CalculatePathRequest() { CalculatePathRequestPool = MemoryBufferStack <CalculatePathRequest> .alloc(() => { return(CalculatePathRequest.alloc()); }); }
/// <summary> /// Path calculation is a recursive algorithm that try all possibilities of <see cref="NavigationLink"/> from the <paramref name="p_beginNode"/> to reach the <paramref name="p_endNode"/>. /// These possibilities are ordered by a score (see <see cref="NavigationNodePathTraversalCalculations"/> for details) that represents the "difficulty" to reach the destination. /// The combinaison of <see cref="NavigationLink"/> that leads to the lower score is the resulting path. /// </summary> public static void CalculatePath(CalculatePathRequest p_request) { if (p_request.BeginNode != null && p_request.EndNode != null) { if (p_request.BeginNode == p_request.EndNode) { p_request.ResultPath.NavigationNodesTraversalCalculations[p_request.BeginNode] = NavigationNodePathTraversalCalculations.build(); return; } NavigationNode l_currentEvaluatedNode = p_request.BeginNode; bool l_isFirstTimeInLoop = true; while (l_currentEvaluatedNode != null && l_currentEvaluatedNode != p_request.EndNode) { // If this is not the start, we find the next current node to pick if (!l_isFirstTimeInLoop) { l_currentEvaluatedNode = pickNextCurrentNodeToCalculate(p_request.ResultPath.NavigationNodesTraversalCalculations, p_request.NodesElligibleForNextCurrent); } else { l_isFirstTimeInLoop = false; NavigationNodePathTraversalCalculations l_pathTraversalCaluclation = NavigationNodePathTraversalCalculations.build(); l_pathTraversalCaluclation.CalculationMadeFrom = l_currentEvaluatedNode; p_request.ResultPath.NavigationNodesTraversalCalculations[l_currentEvaluatedNode] = l_pathTraversalCaluclation; p_request.NodesElligibleForNextCurrent.Add(l_currentEvaluatedNode); } if (l_currentEvaluatedNode != null) { // For the current node, we evaluate the score of it's neighbors // The current evaluated node is no more ellisible because it has just been traversed by the algorithm p_request.NodesElligibleForNextCurrent.Remove(l_currentEvaluatedNode); // If the current evaluated node has links if (p_request.NavigationGraph.NodeLinksIndexedByStartNode.ContainsKey(l_currentEvaluatedNode)) { List <NavigationLink> l_evaluatedNavigationLinks = p_request.NavigationGraph.NodeLinksIndexedByStartNode[l_currentEvaluatedNode]; for (int i = 0; i < l_evaluatedNavigationLinks.Count; i++) { NavigationLink l_link = l_evaluatedNavigationLinks[i]; // We calculate the score as if the linked node is traversed. NavigationNodePathTraversalCalculations l_currentCalculation = p_request.ResultPath.NavigationNodesTraversalCalculations[l_currentEvaluatedNode]; float l_calculatedPathScore = simulateNavigationLinkTraversal(ref l_currentCalculation, ref l_link); // If the neighbor has not already been calculated if (!p_request.ResultPath.NavigationNodesTraversalCalculations.ContainsKey(l_link.EndNode)) { NavigationNode l_linkEndNode = l_link.EndNode; NavigationNodePathTraversalCalculations l_linkNodeCalculations = NavigationNodePathTraversalCalculations.build(); updatePathScore(ref l_linkNodeCalculations, l_calculatedPathScore, l_currentEvaluatedNode); calculateHeuristicScore(ref l_linkNodeCalculations, l_linkEndNode, p_request.EndNode, p_request.PathCalculationParameters.HeurisitcDistanceWeight); p_request.ResultPath.NavigationNodesTraversalCalculations[l_linkEndNode] = l_linkNodeCalculations; p_request.NodesElligibleForNextCurrent.Add(l_linkEndNode); } // Else, we update score calculations only if the current calculated score is lower than the previous one. // This means we have found a less costly path. else { NavigationNode l_linkEndNode = l_link.EndNode; NavigationNodePathTraversalCalculations l_linkNodeCalculations = p_request.ResultPath.NavigationNodesTraversalCalculations[l_linkEndNode]; updatePathScore(ref l_linkNodeCalculations, l_calculatedPathScore, l_currentEvaluatedNode); p_request.ResultPath.NavigationNodesTraversalCalculations[l_linkEndNode] = l_linkNodeCalculations; } } } } } // calculating final path by going to the start by taking "CalculationMadeFrom" nodes if (l_currentEvaluatedNode != null) { p_request.ResultPath.NavigationNodes.Add(l_currentEvaluatedNode); NavigationNodePathTraversalCalculations l_navigationNodePathTraversalCalculations = p_request.ResultPath.NavigationNodesTraversalCalculations[l_currentEvaluatedNode]; p_request.ResultPath.PathCost = NavigationNodePathTraversalCalculations.calculateTotalScore(ref l_navigationNodePathTraversalCalculations); while (l_currentEvaluatedNode != p_request.BeginNode) { NavigationNode l_parent = p_request.ResultPath.NavigationNodesTraversalCalculations[l_currentEvaluatedNode].CalculationMadeFrom; if (l_parent != null) { p_request.ResultPath.NavigationNodes.Add(l_parent); l_currentEvaluatedNode = l_parent; } else { l_currentEvaluatedNode = p_request.BeginNode; } } p_request.ResultPath.NavigationNodes.Reverse(); } } }