/* * Alter NavigationNodeLinks by restoring NavigationLinks following the rule dictated by p_navigationLinkAlterationMethod (see ENavigationLinkRemovalMethod for more). * - NavigationNodeLinks are restored from the NavigationGraphSnapshot. */ public static void restoreNavigationLinksFromSnapshot(NavigationGraph p_navigationGraph, ENavigationLinkAlterationMethod p_navigationLinkAlterationMethod, NavigationNode p_involvedNode) { switch (p_navigationLinkAlterationMethod) { case ENavigationLinkAlterationMethod.TO: // We have to : // * copy NavigationLinks going to the p_involvedNode (retrieved from snapshot) // * add NavigationLinks coming from NavigationNodes that goes to the p_involvedNode List <NavigationLink> l_linksThatPointsTowardsInvolvedNode = p_navigationGraph.NodeLinksIndexedByEndNode[p_involvedNode]; l_linksThatPointsTowardsInvolvedNode.Clear(); l_linksThatPointsTowardsInvolvedNode.AddRange(p_navigationGraph.NavigationGraphSnapshot.NodeLinksIndexedByEndNode[p_involvedNode]); for (int i = 0; i < l_linksThatPointsTowardsInvolvedNode.Count; i++) { NavigationLink l_linkTo = l_linksThatPointsTowardsInvolvedNode[i]; var l_linksFrom_snapshot_enumerator = p_navigationGraph.NavigationGraphSnapshot.NodeLinksIndexedByStartNode[l_linkTo.StartNode].GetEnumerator(); while (l_linksFrom_snapshot_enumerator.MoveNext()) { NavigationLink l_referenceFromLink = l_linksFrom_snapshot_enumerator.Current; if (l_referenceFromLink.EndNode == p_involvedNode) { p_navigationGraph.NodeLinksIndexedByStartNode[l_referenceFromLink.StartNode].Add(l_referenceFromLink); } } } break; } }
public static NavigationLink alloc(NavigationNode p_startNode, NavigationNode p_endNode, float p_travelCost) { NavigationLink l_instance = new NavigationLink(); l_instance.StartNode = p_startNode; l_instance.EndNode = p_endNode; l_instance.TravelCost = p_travelCost; return(l_instance); }
public static void createLinkBetween(NavigationGraph p_navigationGraph, NavigationNode p_startNode, NavigationNode p_endNode, float p_travelCost) { NavigationLink l_instanciatedNavigationLink = NavigationLink.alloc(p_startNode, p_endNode, p_travelCost); if (!p_navigationGraph.NodeLinksIndexedByStartNode.ContainsKey(p_startNode)) { p_navigationGraph.NodeLinksIndexedByStartNode.Add(p_startNode, new List <NavigationLink>()); } if (!p_navigationGraph.NodeLinksIndexedByEndNode.ContainsKey(p_endNode)) { p_navigationGraph.NodeLinksIndexedByEndNode.Add(p_endNode, new List <NavigationLink>()); } p_navigationGraph.NodeLinksIndexedByStartNode[p_startNode].Add(l_instanciatedNavigationLink); p_navigationGraph.NodeLinksIndexedByEndNode[p_endNode].Add(l_instanciatedNavigationLink); }
/* * Alter NavigationNodeLinks by removing the NavigationLinks following the rule dictated by p_navigationLinkAlterationMethod (see ENavigationLinkRemovalMethod for more). */ public static void removeNavigationLinks(NavigationGraph p_navigationGraph, ENavigationLinkAlterationMethod p_navigationLinkAlterationMethod, NavigationNode p_involvedNode) { switch (p_navigationLinkAlterationMethod) { case ENavigationLinkAlterationMethod.TO: // We have to : // * gets NavigationLinks going to the p_involvedNode // * clear NavigationLinks coming from NavigationNodes that goes to the p_involvedNode // * clear NavigationLinks going to the p_involvedNode List <NavigationLink> l_nodesToList = p_navigationGraph.NodeLinksIndexedByEndNode[p_involvedNode]; for (int i = 0; i < l_nodesToList.Count; i++) { NavigationLink l_linkTo = l_nodesToList[i]; List <NavigationLink> l_targetList = p_navigationGraph.NodeLinksIndexedByStartNode[l_linkTo.StartNode]; l_targetList.Remove(l_linkTo); } p_navigationGraph.NodeLinksIndexedByEndNode[p_involvedNode].Clear(); break; } }
/** * Calculates the path score as if the p_traversedNavigationLink_ptr has been traversed. */ private static float simulateNavigationLinkTraversal(ref NavigationNodePathTraversalCalculations p_navigationNodePathTraversalCalculations, ref NavigationLink p_navigationLink) { return(p_navigationNodePathTraversalCalculations.PathScore + p_navigationLink.TravelCost); }
/// <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(); } } }