/// <summary>
/// Unity's Start method. Initializes the NavTileAgent.
/// </summary>
private void Start()
{
OnAreaChangeUnityEvent.AddListener(ChangeAnimationOnArea);
OnPathAbortedUnityEvent.AddListener(OnPathAbortSetAnimationParameters);
// Initialize Conflict handling varaibles when enabled.
if (_conflictOption != EConflictOptions.Disabled)
{
if (!NavTileManager.Instance.SurfaceManager.IsDataInitialized ||
NavTileManager.Instance.SurfaceManager.Data.HasNoTiles ||
NavTileManager.Instance.SurfaceManager.Grid == null)
{
Debug.LogWarning("Data is not initialized for this scene. Please bake in the navigation 2D window.");
return;
}
Vector2Int currentTilePosition = NavTileManager.Instance.SurfaceManager.Grid.WorldToCell(transform.position).GetVector2Int();
NavTileManager.Instance.SurfaceManager.Data.GetTileData(currentTilePosition).OccupyingAgents.Add(this);
_occupyingTilePosition = currentTilePosition;
}
_pathStatus = EPathStatus.NotAvailable;
_movementStatus = EMovementStatus.Stationary;
}
public PathRoute(EPathStatus status, Node source, Node target, Edge[] edges, double distance)
{
ValidateCtor(source, target);
if (edges == null || edges.Length == 0)
{
throw new ArgumentNullException("Edges can't be null or empty");
}
this.Edges = edges;
this.Distance = distance;
this.Status = status;
this.Nodes = new Node[edges.Length + 1];
for (var i = 0; i < edges.Length; i++)
{
var edge = edges[i];
this.Nodes[i] = edge.Source;
this.Nodes[i + 1] = edge.Target;
}
this.Source = source;
this.Target = target;
}
// public PathRoute(Node[] nodes, double distance, EPathStatus status)
// {
// if(nodes == null)
// {
// throw new ArgumentNullException("Nodes can't be null");
// }
// this.Nodes = nodes;
// this.Distance = distance;
// this.Status = status;
// }
public PathRoute(EPathStatus status, Node source, Node target)
{
ValidateCtor(source, target);
this.Status = status;
this.Source = source;
this.Target = target;
}
public void CancelPath()
{
if (_followingPathCoroutine != null)
{
StopCoroutine(_followingPathCoroutine);
_pathStatus = EPathStatus.NotAvailable;
_movementStatus = EMovementStatus.Stationary;
UpdateTileOccupancy(_occupyingTilePosition, NavTileManager.Instance.SurfaceManager.Grid.WorldToCell(transform.position).GetVector2Int());
}
_path = null;
}
public static PathRoute ShortestPathHeuristic(Node source, Node target, Func <Edge, double> edgeWeightCalculation, Func <Node, Node, double> distanceHeuristic)
{
// f(n) = g(n) + h(n)
if (source == null || target == null || edgeWeightCalculation == null || distanceHeuristic == null)
{
throw new ArgumentNullException("No parameter can be null");
}
else if (source == target || source.Id == target.Id)
{
throw new ArgumentException("Source and target must be different");
}
IList <Node> border = new List <Node>();
IDictionary <long, double> weightToNode = new Dictionary <long, double>();
IDictionary <long, double> totalCostForNode = new Dictionary <long, double>();
IDictionary <long, Edge> parents = new Dictionary <long, Edge>();
int quantityOfExpansions = 0;
IList <long> searched = new List <long>();
border.Add(source);
weightToNode.Add(source.Id, 0);
totalCostForNode.Add(source.Id, distanceHeuristic(source, target));
Node current = null;
while (border.Count != 0)
{
current = border[0];
border.RemoveAt(0);
quantityOfExpansions++;
if (current == target)
{
break;
}
var currentWeight = weightToNode[current.Id];
IList <Node> childrens = null;
childrens = current.NeighborsOut();
foreach (Node children in childrens)
{
var bestRouteToChildren = Node.ShortestPathBetweenNeihbors(current, children, edgeWeightCalculation);
if (bestRouteToChildren.Status == EPathStatus.NotFound)
{
throw new Exception("Expanded node lost reference to children");
}
Edge edge = bestRouteToChildren.Edges[0];
var weight = bestRouteToChildren.Distance;
double weightToChildren = currentWeight + weight;
double costToTarget = distanceHeuristic(children, target);
double costFunction = weightToChildren + costToTarget;
if (weightToNode.ContainsKey(children.Id))
{
if (weightToNode[children.Id] <= weightToChildren)
{
continue;
}
}
else
{
border.Remove(children);
}
weightToNode[children.Id] = weightToChildren;
totalCostForNode[children.Id] = costFunction;
parents[children.Id] = edge;
var index = border.FindIndex(node => totalCostForNode[node.Id] > costFunction);
border.Insert(index >= 0 ? index : border.Count, children);
}
}
if (current == target)
{
IList <Edge> edges = new List <Edge>();
EPathStatus status = EPathStatus.Found;
var edge = parents[target.Id];
edges.Add(edge);
Node parent = edge.Source;
while (parent != source)
{
edge = parents[parent.Id];
edges.Add(edge);
if (edge == null || edge.Source == null)
{
status = EPathStatus.FailOnRouteBuilding;
break;
}
parent = edge.Source;
}
var pathRoute = new PathRoute(status, source, target, edges.Reverse().ToArray(), weightToNode[target.Id]);
pathRoute.QuantityOfExpansions = quantityOfExpansions;
return(pathRoute);
}
return(new PathRoute(EPathStatus.NotFound, source, target));
}
public RouteMeasures(EPathStatus Status)
{
this.Status = Status;
}
/// <summary>
/// Coroutine for following a given path.
/// </summary>
/// <param name="inPath">The path that should be followed.</param>
private IEnumerator FollowPath(NavTilePath inPath)
{
if (inPath == null)
{
yield break;
}
// Prepare path traversal.
_currentPathNodeIndex = 0;
PathNode currentNode = new PathNode(transform.position);
PathNode nextNode = inPath[_currentPathNodeIndex];
_pathStatus = EPathStatus.Traversing;
_movementStatus = EMovementStatus.Traversing;
float timeInPath = 0;
float timeInCurrentNode = 0;
float currentSpeed = GetAreaSpeed(nextNode.GetAreaIndex());
OnAreaChangeUnityEvent.Invoke(nextNode.GetAreaIndex());
float timeToNextNode = inPath.GetDurationBetweenNodes(currentNode, nextNode, currentSpeed);
// Initial animation triggers.
SetAnimationParameters(nextNode.WorldPosition - currentNode.WorldPosition, currentSpeed);
// Do conflict handling if enabled
if (_conflictOption != EConflictOptions.Disabled)
{
// Check if there is a conflict before starting to traverse.
EConflictStatus conflictStatus;
EAbortReason abortReason;
do
{
conflictStatus = HandleConflict(currentNode, nextNode, out abortReason);
// In case the conflict is not completely resolved, yield the coroutine and check again next frame.
if (conflictStatus == EConflictStatus.Processing)
{
_movementStatus = EMovementStatus.Waiting;
yield return(null);
}
else if (conflictStatus == EConflictStatus.Abort) // In case the conflict cannot be resolved, abort path traversing.
{
_pathStatus = EPathStatus.NotAvailable;
_movementStatus = EMovementStatus.Stationary;
OnPathAbortedUnityEvent.Invoke(abortReason, nextNode.TilePosition);
yield break;
}
} while (conflictStatus == EConflictStatus.Processing); // If this is false, the 'do-while' will stop and thus a conflict has not been encountered.
}
// Start traversing.
while (true)
{
// Check if the agent will reach the next node in this frame or not.
// If so, start walking towards the subsequent node.
if (timeInCurrentNode + Time.deltaTime >= timeToNextNode)
{
// Switch to next node.
_currentPathNodeIndex++;
// Goal reached?
if (_currentPathNodeIndex == inPath.Count)
{
transform.position = nextNode.WorldPosition;
_pathStatus = EPathStatus.NotAvailable;
_movementStatus = EMovementStatus.Stationary;
// Reset animation parameters.
ResetAnimationParameters();
OnPathFinishedUnityEvent.Invoke();
yield break;
}
currentNode = nextNode;
nextNode = inPath[_currentPathNodeIndex];
// The 'previous' position is the tile which the agent is walking from.
_previousPosition = currentNode.TilePosition;
// Check if next tile is still walkable.
if (!NavTileManager.Instance.SurfaceManager.Data.IsTileWalkable(nextNode.TilePosition, AreaMask))
{
OnPathAbortedUnityEvent.Invoke(EAbortReason.EncounteredObstruction, nextNode.TilePosition);
yield break;
}
// Check for conflicts if enabled.
if (_conflictOption != EConflictOptions.Disabled)
{
// Check if there is a conflict on the next node.
EConflictStatus conflictStatus;
EAbortReason abortReason;
do
{
conflictStatus = HandleConflict(currentNode, nextNode, out abortReason);
// In case the conflict is not completely resolved, yield the coroutine and check again next frame.
if (conflictStatus == EConflictStatus.Processing)
{
_movementStatus = EMovementStatus.Waiting;
yield return(null);
}
else if (conflictStatus == EConflictStatus.Abort) // In case the conflict cannot be resolved, abort path traversing.
{
_pathStatus = EPathStatus.NotAvailable;
_movementStatus = EMovementStatus.Stationary;
OnPathAbortedUnityEvent.Invoke(abortReason, nextNode.TilePosition);
yield break;
}
} while (conflictStatus == EConflictStatus.Processing); // If this is false, the 'do-while' will stop and thus a conflict has not been encountered.
// Set movementstatus just in case it has been changed due to a conflict.
_movementStatus = EMovementStatus.Traversing;
}
// Check whether the agent has changed areas.
int nextAreaIndex = nextNode.GetAreaIndex();
if (currentNode.GetAreaIndex() != nextAreaIndex)
{
currentSpeed = GetAreaSpeed(nextAreaIndex);
OnAreaChangeUnityEvent.Invoke(nextAreaIndex);
}
// Calculate the time which might be left over from the previous node to already move in towards the subsequent node.
// This prevents the agent to first exactly stop on a tile before starting to move again, creating jittering.
timeInCurrentNode += Time.deltaTime - timeToNextNode;
timeToNextNode = inPath.GetDurationBetweenNodes(currentNode, nextNode, currentSpeed);
transform.position = Vector3.Lerp(currentNode.WorldPosition, nextNode.WorldPosition, timeInCurrentNode / timeToNextNode);
// Set animation parameters
SetAnimationParameters(nextNode.WorldPosition - currentNode.WorldPosition, currentSpeed);
yield return(null);
}
else // The next node has not been reached, so keep moving towards it.
{
timeInPath += Time.deltaTime;
timeInCurrentNode += Time.deltaTime;
transform.position = Vector3.Lerp(currentNode.WorldPosition, nextNode.WorldPosition, timeInCurrentNode / timeToNextNode);
yield return(null);
}
}
}
/// <summary>
/// Moves the agent to a tile position
/// </summary>
/// <param name="inTargetPosition">The position to move to as a tile position.</param>
public async void MoveToPosition(Vector2Int inTargetPosition, params Vector2Int[] inAvoidTiles)
{
// Check for early exits or possible errors first
if (NavTileManager.Instance.SurfaceManager.Grid == null)
{
Debug.LogWarning("There is no (or multiple) grid object(s) found in the scene. Path can't be calculated without one.");
OnPathNotFoundUnityEvent.Invoke();
return;
}
if (!NavTileManager.Instance.SurfaceManager.IsDataInitialized || NavTileManager.Instance.SurfaceManager.Data.HasNoTiles)
{
Debug.LogWarning("Data is not initialized for this scene. Please bake in the navigation 2D window.");
OnPathNotFoundUnityEvent.Invoke();
return;
}
Vector2Int startCoordinate = NavTileManager.Instance.SurfaceManager.Grid.WorldToCell(this.transform.position).GetVector2Int();
if (startCoordinate == inTargetPosition)
{
Debug.LogWarning("Trying to find a path to the start node. Calculating a path is redundant.");
OnPathNotFoundUnityEvent.Invoke();
return;
}
if (NavTileManager.Instance.SurfaceManager.Data.GetTileData(inTargetPosition) == null)
{
Debug.LogWarning(string.Format("Coordinate {0} is not a tile on the grid, can't calculate path.", inTargetPosition));
OnPathNotFoundUnityEvent.Invoke();
return;
}
// Start calculating a path.
_pathStatus = EPathStatus.Calculating;
// Intialize parameters to find a path.
FindPathInput input = new FindPathInput(startCoordinate, inTargetPosition, _areaMask, this._diagonalAllowed, this._cutCorners, this._ignoreTileCost, inAvoidTiles);
#if UNITY_EDITOR
input.Visualizer = visualizer;
#endif
// Get a path based on the input and the Pipeline Settings in the Navigation2D window.
_path = await NavTileManager.Instance.GetPath(input);
// If no path is found, return.
if (_path == null || _path.Count == 0)
{
Debug.LogWarning("Path could not be found.", gameObject);
_pathStatus = EPathStatus.NotAvailable;
OnPathNotFoundUnityEvent.Invoke();
return;
}
OnPathFound();
}
