/*
* 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;
}
}
/// <summary>
/// Because <see cref="NavigationNode.LocalPosition"/> is in <see cref="NavigationGraph"/> local space, all positions must be converted to world space from this component.
/// As the <see cref="NavigationGraphComponent"/> is the holder of the <see cref="m_navigationGraph"/> it is his role.
/// </summary>
/// <returns> <paramref name="p_navigationNode"/> position in world space.</returns>
public static float3 get_WorldPositionFromNavigationNode(NavigationGraphComponent p_navigationGraphComponent, NavigationNode p_navigationNode)
{
return(p_navigationGraphComponent.transform.localToWorldMatrix.MultiplyPoint(p_navigationNode.LocalPosition));
}
/*
* 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;
}
}
/// <summary>
/// Checks if the <paramref name="p_requestedNode1"/> and <paramref name="p_requestedNode2"/> have a direct <see cref="NavigationLink"/> connection.
/// The checked <see cref="NavigationLink"/> is oriented from <paramref name="p_requestedNode1"/> to <paramref name="p_requestedNode2"/>.
/// </summary>
public static bool areNavigationNodesNeighbors(NavigationGraph p_navigationGraph, NavigationNode p_requestedNode1, NavigationNode p_requestedNode2, NavigationGraphFlag p_navigationGraphFlag)
{
foreach (NavigationNode l_reachableNavigationNode in getReachableNeighborNavigationNodes(p_navigationGraph, p_requestedNode1, p_navigationGraphFlag))
{
if (l_reachableNavigationNode == p_requestedNode2)
{
return(true);
}
}
return(false);
}
/// <summary>
/// Find all NavigationNodes that the <paramref name="p_requestedNode"/> can travel to and have direct <see cref="NavigationLink"/> between them.
/// - "Reachable" : Means that it exists a path between the <paramref name="p_requestedNode"/> and the returned NavigationNode
/// - "Neighbor" : Means that the <paramref name="p_requestedNode"/> and the returned NavigationNode have a direct NavigationLink between them.
/// </summary>
public static IEnumerable <NavigationNode> getReachableNeighborNavigationNodes(NavigationGraph p_navigationGraph, NavigationNode p_requestedNode, NavigationGraphFlag p_navigationGraphFlag)
{
var l_navigationLinksGoingFromTheRequestedNode = NavigationGraph.get_nodeLinksIndexedByStartNode(p_navigationGraph, p_navigationGraphFlag)[p_requestedNode];
var l_navigationLinksGoingFromTheRequestedNodeEnumerator = l_navigationLinksGoingFromTheRequestedNode.GetEnumerator();
while (l_navigationLinksGoingFromTheRequestedNodeEnumerator.MoveNext())
{
yield return(l_navigationLinksGoingFromTheRequestedNodeEnumerator.Current.EndNode);
}
}
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;
}
/// <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();
}
}
}
