예제 #1
0
        /**
         *   The picked NavigationNode with the lowest total score (retrieved from NavigationNodePathTraversalCalculations) is returned
         */
        private static NavigationNode pickNextCurrentNodeToCalculate(
            Dictionary <NavigationNode, NavigationNodePathTraversalCalculations> l_pathScoreCalculations,
            List <NavigationNode> l_pathNodesElligibleForNextCurrent)
        {
            NavigationNode l_currentSelectedNode = null;
            float          l_currentTotalScore   = 0.0f;

            for (int i = 0; i < l_pathNodesElligibleForNextCurrent.Count; i++)
            {
                NavigationNode l_currentComparedNavigationnode = l_pathNodesElligibleForNextCurrent[i];

                if (l_currentSelectedNode == null)
                {
                    l_currentSelectedNode = l_currentComparedNavigationnode;
                    NavigationNodePathTraversalCalculations l_navigationNodePathTraversalCalculations = l_pathScoreCalculations[l_currentComparedNavigationnode];
                    l_currentTotalScore = NavigationNodePathTraversalCalculations.calculateTotalScore(ref l_navigationNodePathTraversalCalculations);
                }
                else
                {
                    NavigationNodePathTraversalCalculations l_navigationNodePathTraversalCalculations = l_pathScoreCalculations[l_currentComparedNavigationnode];
                    float l_currentComparedTotalScore = NavigationNodePathTraversalCalculations.calculateTotalScore(ref l_navigationNodePathTraversalCalculations);
                    if (l_currentComparedTotalScore < l_currentTotalScore)
                    {
                        l_currentTotalScore   = l_currentComparedTotalScore;
                        l_currentSelectedNode = l_currentComparedNavigationnode;
                    }
                }
            }

            return(l_currentSelectedNode);
        }
예제 #2
0
 /**
  *  The Heuristic score is the distance between the associated NavigationNode and the target of the path algorithm.
  */
 private static void calculateHeuristicScore(
     ref NavigationNodePathTraversalCalculations p_navigationNodePathTraversalCalculations,
     NavigationNode p_startNode,
     NavigationNode p_endNode,
     float p_heurisitcDistanceMultiplier)
 {
     p_navigationNodePathTraversalCalculations.HeuristicScore =
         math.distance(p_startNode.LocalPosition, p_endNode.LocalPosition) * p_heurisitcDistanceMultiplier;
 }
예제 #3
0
        public static NavigationNodePathTraversalCalculations build()
        {
            NavigationNodePathTraversalCalculations l_instance = new NavigationNodePathTraversalCalculations();

            l_instance.PathScore      = 0.0f;
            l_instance.HeuristicScore = 0.0f;
            l_instance.IsAlreadyEvaluatedByTheAlgorithm = false;
            l_instance.CalculationMadeFrom = null;
            return(l_instance);
        }
예제 #4
0
 /**
  *  When the caller wants to update the m_pathScore with an already calculated Path score (p_newPathScore) adn from which NavigationNode this calculation has been done (p_calculatedFrom_ptr).
  *  p_newPathScore andp_calculatedFrom_ptr is keeped only if the p_newPathScore is lower than the current one.
  */
 private static void updatePathScore(
     ref NavigationNodePathTraversalCalculations p_navigationNodePathTraversalCalculations,
     float p_newPathScore,
     NavigationNode p_calculatedFrom)
 {
     if (p_navigationNodePathTraversalCalculations.CalculationMadeFrom == null || p_newPathScore < p_navigationNodePathTraversalCalculations.PathScore)
     {
         p_navigationNodePathTraversalCalculations.PathScore           = p_newPathScore;
         p_navigationNodePathTraversalCalculations.CalculationMadeFrom = p_calculatedFrom;
     }
 }
예제 #5
0
 /** The total score is the sum of m_pathScore and m_heuristicScore. */
 public static float calculateTotalScore(ref NavigationNodePathTraversalCalculations p_navigationNodePathTraversalCalculations)
 {
     return(p_navigationNodePathTraversalCalculations.PathScore + p_navigationNodePathTraversalCalculations.HeuristicScore);
 }
예제 #6
0
 /**
  *  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);
 }
예제 #7
0
        /// <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();
                }
            }
        }