/**
* 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);
}
/**
* 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;
}
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);
}
/**
* 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;
}
}
/** 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);
}
/**
* 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();
}
}
}
