public bool PreProcess(IPathRequest request)
{
if (!_enabled)
{
return(false);
}
var grid = GridManager.instance.GetGrid(request.to);
if (grid == null)
{
return(false);
}
var goal = grid.GetCell(request.to, true);
if (IsEvenIndex(goal))
{
request.to = goal.position;
return(true);
}
int count = goal.GetNeighbours(_neighbourBuffer);
for (int i = 0; i < count; i++)
{
if (IsEvenIndex(_neighbourBuffer[i]))
{
request.to = _neighbourBuffer[i].position;
return(true);
}
}
return(false);
}
/// <summary>
/// Requests the path.
/// </summary>
/// <param name="request">The request.</param>
public void RequestPath(IPathRequest request)
{
var unit = this.GetUnitFacade();
request = InternalPathRequest.Internalize(request);
request.timeStamp = Time.time;
lock (_syncLock)
{
request.requester = this;
request.requesterProperties = unit;
if (_pendingPathRequest != null)
{
_pendingPathRequest.hasDecayed = true;
}
_pendingPathRequest = request;
_stop = false;
_stopped = false;
}
GameServices.pathService.QueueRequest(_pendingPathRequest, unit.pathFinderOptions.pathingPriority);
}
public NodePath FindPath(IPathfindNodeNetwork <AstarNode> nodeNetwork, IPathRequest pathRequest, out bool succes)
{
var pathfindingNetwork = nodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory);
var startNode = NodePointer.Dereference(pathRequest.PathStart.Index, pathfindingNetwork);
var endNode = NodePointer.Dereference(pathRequest.PathEnd.Index, pathfindingNetwork);
var path = FindPath(pathfindingNetwork, startNode, endNode, pathRequest.AgentSize, pathRequest.CollisionCategory);
if (path == null)
{
succes = false;
return(new NodePath(new[] { startNode.DefinitionNode }, nodeNetwork.DefinitionNodeNetwork.Transformer));
}
succes = true;
switch (nodeNetwork.DefinitionNodeNetwork)
{
case IDefinitionNodeGrid definitionNodeGrid:
var offset = GridClearanceHelper.GridNodeOffset(pathRequest.AgentSize, definitionNodeGrid.Transformer.Scale);
return(new NodePath(path.ToArray(), definitionNodeGrid.Transformer));
case IDefinitionNodeNetwork definitionNodeNetwork:
return(new NodePath(path.ToArray(), definitionNodeNetwork.Transformer));
default:
throw new NotSupportedException($"{nodeNetwork.DefinitionNodeNetwork.GetType()} is not supported");
}
}
/// <summary>
/// Initializes a new instance of the <see cref="PathResult"/> class.
/// </summary>
/// <param name="status">The status.</param>
/// <param name="path">The path.</param>
/// <param name="pathCost">The cost of the path, i.e. its length and combined cost of the cells involved</param>
/// <param name="originalRequest">The original request.</param>
public PathResult(PathingStatus status, Path path, int pathCost, IPathRequest originalRequest)
{
this.status = status;
this.path = path ?? _pathEmpty;
this.pathCost = pathCost;
this.originalRequest = originalRequest;
}
public NodePath FindPath(IPathfindNodeNetwork <AstarNode> nodeNetwork, IPathRequest pathRequest, out bool succes)
{
if (pathRequest.PathStart == pathRequest.PathEnd)
{
succes = true;
return(NodePath.GetEmptyPath(nodeNetwork, pathRequest.PathStart));
}
var pathfindingNetwork = nodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory);
if (!(pathfindingNetwork[pathRequest.PathStart].Clearance >= pathRequest.AgentSize) || !(pathfindingNetwork[pathRequest.PathEnd].Clearance >= pathRequest.AgentSize))
{
succes = false;
return(NodePath.GetEmptyPath(nodeNetwork, pathRequest.PathStart));
}
StartFindPath(pathfindingNetwork, nodeNetwork.DefinitionNodeNetwork.NodeArray, pathRequest.PathStart);
if (FindPath(pathfindingNetwork, nodeNetwork.DefinitionNodeNetwork.NodeArray, pathRequest.PathEnd, pathRequest.AgentSize, pathRequest.CollisionCategory))
{
var path = _pathRetracer.RetracePath(pathfindingNetwork, nodeNetwork.DefinitionNodeNetwork.NodeArray, pathRequest.PathStart, pathRequest.PathEnd);
succes = true;
return(new NodePath(nodeNetwork.DefinitionNodeNetwork.NodeArray, path, nodeNetwork.DefinitionNodeNetwork.Transformer));
}
succes = false;
return(NodePath.GetEmptyPath(nodeNetwork, pathRequest.PathStart));
}
/// <summary>
/// Initializes a new instance of the <see cref="PathResult"/> class.
/// </summary>
/// <param name="status">The status.</param>
/// <param name="path">The path.</param>
/// <param name="pathCost">The cost of the path, i.e. its length and combined cost of the cells involved</param>
/// <param name="originalRequest">The original request.</param>
public PathResult(PathingStatus status, Path path, int pathCost, IPathRequest originalRequest)
{
this.status = status;
this.path = path ?? _pathEmpty;
this.pathCost = pathCost;
this.originalRequest = originalRequest;
}
public bool PreProcess(IPathRequest request)
{
if (!_enabled)
{
return(false);
}
var grid = GridManager.instance.GetGrid(request.to);
if (grid == null)
{
return(false);
}
var goal = grid.GetCell(request.to, true);
if (IsEvenIndex(goal))
{
request.to = goal.position;
return(true);
}
var best = _eval.ClosestMatch(grid, request.to, IsEvenIndex, Discard);
request.to = best.position;
return(true);
}
private void RequestPath(Vector3 from, Vector3 to, InternalPathRequest.Type type)
{
var unit = this.GetUnitFacade();
lock (_syncLock)
{
if (_pendingPathRequest != null)
{
_pendingPathRequest.hasDecayed = true;
}
_pendingPathRequest = new InternalPathRequest
{
from = from,
to = to,
pathType = type,
requester = this,
requesterProperties = unit,
pathFinderOptions = unit.pathFinderOptions,
timeStamp = Time.time
};
if (type == InternalPathRequest.Type.Normal)
{
_pendingPathRequest.via = _wayPoints.GetViaPoints();
}
_stop = false;
_stopped = false;
}
GameServices.pathService.QueueRequest(_pendingPathRequest, unit.pathFinderOptions.pathingPriority);
}
/// <summary>
/// Processes the request.
/// </summary>
/// <param name="request">The request.</param>
public void ProcessRequest(IPathRequest request)
{
try
{
//Granted this could be done smarter, but since this is not an expected scenario, we just use exception handling to handle it.
ValidateRequest(request);
_currentResult = new PathResult(PathingStatus.Complete, null, 0, request);
var status = PathingStatus.Complete;
//Iterate over the waypoints of the request, including the end point
_segmentRequest.Start(request);
do
{
status = StartPathSegment();
if (status != PathingStatus.Running)
{
break;
}
while (status == PathingStatus.Running)
{
status = ProcessNext();
}
if (!CompletePathSegment(status))
{
break;
}
}while (_segmentRequest.MoveNext());
if (status != PathingStatus.Complete)
{
if (_segments.count > 0)
{
_currentResult.RegisterPartialResult(status, _segmentRequest.GetPendingWaypoints());
}
else
{
_currentResult.status = status;
}
}
//Splice the path segments and complete the request
_currentResult.path = Path.FromSegments(_segments);
request.Complete(_currentResult);
}
catch (Exception e)
{
FailRequest(request, e);
}
finally
{
_segments.Clear();
_segmentRequest.Clear();
_currentResult = null;
}
}
public override bool PreProcess(IPathRequest request)
{
var toGrid = GridManager.instance.GetGrid(request.to);
var fromGrid = GridManager.instance.GetGrid(request.from);
if ((fromGrid != null && toGrid != null) || (fromGrid == null && toGrid == null))
{
return(false);
}
if (toGrid == null)
{
var goal = fromGrid.GetCell(request.to, true);
request.customData = new PathAddition
{
prepend = false,
point = request.to
};
request.to = goal.position;
}
else
{
var start = toGrid.GetCell(request.from, true);
request.customData = new PathAddition
{
prepend = true,
point = request.from
};
request.from = start.position;
}
return(true);
}
public bool PreProcess(IPathRequest request)
{
if (!_enabled)
{
return(false);
}
var grid = GridManager.instance.GetGrid(request.to);
if (grid == null)
{
return(false);
}
if (SafeHavens.IsSafe(request.to))
{
return(false);
}
var data = new EvalData(request.requesterProperties);
var best = _eval.ClosestMatch(grid, request.to, data.IsItSafe, data.IsItBlocked);
if (best == null)
{
return(false);
}
request.to = best.position;
return(true);
}
public PotentialField FindPath(DijkstraNodeGrid dijkstraNodeNetwork, IPathRequest pathRequest)
{
var pathfindingNetwork = dijkstraNodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory);
_dijkstraAlgorithm.Start(pathfindingNetwork, dijkstraNodeNetwork.DefinitionNodeGrid.NodeArray, pathRequest.PathEnd, pathRequest.PathStart, pathRequest.AgentSize, pathRequest.CollisionCategory);
_dijkstraAlgorithm.Step(-1);
return(FindPath(dijkstraNodeNetwork, pathfindingNetwork, pathRequest.PathEnd, pathRequest));
}
/// <summary>
/// Resolves the direct path or delegates the request on to path finding.
/// </summary>
/// <param name="req">The request.</param>
/// <returns>A path request to use in path finding or null if the path was resolved.</returns>
public IPathRequest ResolveDirectPath(IPathRequest req)
{
var to = req.to;
var from = req.from;
var toGrid = req.toGrid;
var fromGrid = req.fromGrid;
var unitProps = req.requesterProperties;
if (fromGrid == null)
{
fromGrid = req.fromGrid = GridManager.instance.GetGrid(from);
}
if (toGrid == null)
{
toGrid = req.toGrid = GridManager.instance.GetGrid(to);
//Just treat the to grid as the from grid in this case so the destination can be closest point on from grid
if (toGrid == null)
{
toGrid = req.toGrid = fromGrid;
}
}
//If no grids were resolved for this request it means the request involves two points outside the grid(s) that do not cross any grid(s), so we can move directly between them
if (fromGrid == null && toGrid == null)
{
req.Complete(new DirectPathResult(from, to, req));
return null;
}
else if (fromGrid == null || toGrid == null)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return null;
}
//Is the start node on a grid
Cell fromCell = fromGrid.GetCell(from, true);
//If the from cell is blocked, we have to get out to a free cell first
if (!fromCell.isWalkable(unitProps.attributes))
{
//Check for a free cell to escape to before resuming the planned path. If no free cell is found we are stuck.
//The unit may still get stuck even if a free cell is found, in case there is a physically impassable blockade
var escapeCell = fromGrid.GetNearestWalkableCell(from, from, true, req.pathFinderOptions.maxEscapeCellDistanceIfOriginBlocked, unitProps);
if (escapeCell == null)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return null;
}
//Move to the free cell and then on with the planned path
req.Complete(DirectPathResult.CreateWithPath(from, escapeCell.position, to, req));
return null;
}
return req;
}
/// <summary>
/// Resolves the direct path or delegates the request on to path finding.
/// </summary>
/// <param name="req">The request.</param>
/// <returns>A path request to use in path finding or null if the path was resolved.</returns>
public IPathRequest ResolveDirectPath(IPathRequest req)
{
var to = req.to;
var from = req.from;
var toGrid = req.toGrid;
var fromGrid = req.fromGrid;
var unitProps = req.requesterProperties;
if (fromGrid == null)
{
fromGrid = req.fromGrid = GridManager.instance.GetGrid(from);
}
if (toGrid == null)
{
toGrid = req.toGrid = GridManager.instance.GetGrid(to);
//Just treat the to grid as the from grid in this case so the destination can be closest point on from grid
if (toGrid == null)
{
toGrid = req.toGrid = fromGrid;
}
}
//If no grids were resolved for this request it means the request involves two points outside the grid(s) that do not cross any grid(s), so we can move directly between them
if (fromGrid == null && toGrid == null)
{
req.Complete(new DirectPathResult(from, to, req));
return(null);
}
else if (fromGrid == null || toGrid == null)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return(null);
}
//Is the start node on a grid
Cell fromCell = fromGrid.GetCell(from, true);
//If the from cell is blocked, we have to get out to a free cell first
if (!fromCell.isWalkable(unitProps.attributes))
{
//Check for a free cell to escape to before resuming the planned path. If no free cell is found we are stuck.
//The unit may still get stuck even if a free cell is found, in case there is a physically impassable blockade
var escapeCell = fromGrid.GetNearestWalkableCell(from, from, true, req.pathFinderOptions.maxEscapeCellDistanceIfOriginBlocked, unitProps);
if (escapeCell == null)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return(null);
}
//Move to the free cell and then on with the planned path
req.Complete(DirectPathResult.CreateWithPath(from, escapeCell.position, to, req));
return(null);
}
return(req);
}
private void IssueRequest(IPathRequest request, SteerForPathComponent steerer)
{
var action = new OneTimeAction((ignored) =>
{
steerer.RequestPath(request);
});
NavLoadBalancer.defaultBalancer.Add(action, this.retryDelay, true);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathResult"/> class.
/// </summary>
/// <param name="status">The status.</param>
/// <param name="path">The path.</param>
/// <param name="pathCost">The cost of the path, i.e. its length and combined cost of the cells involved</param>
/// <param name="originalRequest">The original request.</param>
public PathResult(PathingStatus status, Path path, int pathCost, IPathRequest originalRequest)
: this()
{
this.status = status;
this.path = path ?? _pathEmpty;
this.pathCost = pathCost;
this.originalRequest = originalRequest;
this.pendingWaypoints = originalRequest.pendingWaypoints;
}
/// <summary>
/// Initializes a new instance of the <see cref="PathResult"/> class.
/// </summary>
/// <param name="status">The status.</param>
/// <param name="path">The path.</param>
/// <param name="pathCost">The cost of the path, i.e. its length and combined cost of the cells involved</param>
/// <param name="originalRequest">The original request.</param>
public PathResult(PathingStatus status, Path path, int pathCost, IPathRequest originalRequest)
: this()
{
this.status = status;
this.path = path ?? _pathEmpty;
this.pathCost = pathCost;
this.originalRequest = originalRequest;
this.pendingWaypoints = originalRequest.pendingWaypoints;
}
/// <summary>
/// Initializes a new instance of the <see cref="DirectPathResult"/> class.
/// </summary>
/// <param name="from">Moving from.</param>
/// <param name="to">Robing to.</param>
/// <param name="originalRequest">The original request.</param>
public DirectPathResult(Vector3 from, Vector3 to, IPathRequest originalRequest)
{
this.path = new Path(2);
this.path.Push(new Position(to));
this.path.Push(new Position(from));
this.originalRequest = originalRequest;
this.status = PathingStatus.Complete;
}
/// <summary>
/// Initializes a new instance of the <see cref="DirectPathResult"/> class.
/// </summary>
/// <param name="from">Moving from.</param>
/// <param name="to">Robing to.</param>
/// <param name="originalRequest">The original request.</param>
public DirectPathResult(Vector3 from, Vector3 to, IPathRequest originalRequest)
{
this.path = new Path(2);
this.path.Push(new Position(to));
this.path.Push(new Position(from));
this.originalRequest = originalRequest;
this.status = PathingStatus.Complete;
}
protected override void ValidateRequest(IPathRequest request)
{
base.ValidateRequest(request);
if (request.pathFinderOptions.preventDiagonalMoves)
{
throw new NotSupportedException("Jump Point Search does not support path requests set to Prevent Diagonal Moves.");
}
}
private void StopInternal()
{
_stopped = true;
_wayPoints.Clear();
_currentPath = null;
_pendingPathRequest = null;
_pendingResult = null;
_manualReplan = null;
}
private void IssueRequest(IPathRequest request, SteerForPathComponent steerer)
{
var action = new OneTimeAction((ignored) =>
{
steerer.RequestPath(request);
});
NavLoadBalancer.defaultBalancer.Add(action, this.retryDelay, true);
}
private void FailRequest(IPathRequest request, Exception e)
{
var result = new PathResult(PathingStatus.Failed, null, 0, request)
{
errorInfo = string.Concat(e.Message, Environment.NewLine, e.StackTrace)
};
request.Complete(result);
_currentRequest = null;
}
public FlowField FindPath(DijkstraNodeGrid dijkstraNodeNetwork, IPathRequest pathRequest, out bool succes)
{
if (_flowFieldCache == null || !_flowFieldCache.TryGetValue(pathRequest, out var flowField))
{
var potentialField = _potentialFieldAlgorithm.FindPath(dijkstraNodeNetwork, pathRequest, out succes);
flowField = new FlowField(potentialField);
_flowFieldCache?.Add(pathRequest, flowField);
}
succes = flowField[pathRequest.PathStart].Length > 0;
return(flowField);
}
private void CompleteRequest(PathingStatus status)
{
PathResult result;
if (_currentRequest.type == RequestType.IntelOnly)
{
result = new PathResult(status, null, _goal.g, _currentRequest);
}
else if (status == PathingStatus.Complete)
{
Path path;
var maxPathLength = Mathf.CeilToInt(_goal.g / (_goal.parent.cellSize * _costProvider.baseMoveCost));
//Fix the actual destination so it does not overlap obstructions
FixupGoal(_currentRequest);
if (_currentRequest.pathFinderOptions.usePathSmoothing)
{
path = _smoother.Smooth(_goal, maxPathLength, _currentRequest, _cellCostStrategy);
}
else
{
path = new Path(maxPathLength);
//Push the actual end position as the goal
path.Push(new Position(_currentRequest.to));
IPathNode current = _goal.predecessor;
while (current != null)
{
path.Push(current);
current = current.predecessor;
}
//Instead of testing for it in the while loop, just pop off the start node and replace it with the actual start position
if (path.count > 1)
{
path.Pop();
}
path.Push(new Position(_currentRequest.from));
}
result = new PathResult(status, path, _goal.g, _currentRequest);
}
else
{
result = new PathResult(status, null, 0, _currentRequest);
}
_currentRequest.Complete(result);
_currentRequest = null;
}
public PotentialField FindPath(DijkstraNodeGrid dijkstraNodeNetwork, IPathRequest pathRequest, out bool succes)
{
if (_potentialFieldCache == null || !_potentialFieldCache.TryGetValue(pathRequest, out var potentialField))
{
var pathfindingNetwork = dijkstraNodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory);
_dijkstraAlgorithm.StartFindPath(pathfindingNetwork, dijkstraNodeNetwork.DefinitionNodeGrid.NodeArray, pathRequest.PathEnd);
_dijkstraAlgorithm.FindPath(pathfindingNetwork, dijkstraNodeNetwork.DefinitionNodeGrid.NodeGrid.Array, pathRequest.PathStart, pathRequest.AgentSize, pathRequest.CollisionCategory);
potentialField = FindPath(dijkstraNodeNetwork, pathfindingNetwork, pathRequest.PathEnd, pathRequest);
_potentialFieldCache?.Add(pathRequest, potentialField);
}
ref var startDefinitionNode = ref dijkstraNodeNetwork.DefinitionNodeGrid.NodeArray[pathRequest.PathStart];
internal static InternalPathRequest Internalize(IPathRequest request)
{
var internalized = request as InternalPathRequest;
if (internalized == null)
{
internalized = new InternalPathRequest();
Utils.CopyProps(request, internalized);
}
internalized.pathType = Type.Normal;
return(internalized);
}
//Note this may be called from another thread if the PathService is running asynchronously
void INeedPath.ConsumePathResult(PathResult result)
{
lock (_syncLock)
{
//If we have stooped or get back the result of a request other than the one we currently expect, just toss it as it will be outdated.
if (_stopped || result.originalRequest != _pendingPathRequest)
{
return;
}
_pendingResult = result;
_pendingPathRequest = null;
}
}
private void ValidateRequest(IPathRequest request)
{
Ensure.ArgumentNotNull(request, "request");
if (!request.isValid)
{
throw new ArgumentException("The request is invalid.", "request");
}
if (_segmentRequest.isRunning)
{
throw new InvalidOperationException("A new request cannot be started while another request is being processed.");
}
}
/// <summary>
/// Queues a request with a priority. Higher values take priority.
/// </summary>
/// <param name="request">The request.</param>
/// <param name="priority">The priority.</param>
public void QueueRequest(IPathRequest request, int priority)
{
Ensure.ArgumentNotNull(request, "request");
lock (_queue)
{
_queue.Enqueue(request, priority);
if (this.runAsync && !_processingActive)
{
StartAsyncProcessing();
}
}
}
private bool StartRequest(IPathRequest request)
{
Ensure.ArgumentNotNull(request, "request");
if (!request.isValid)
{
throw new ArgumentException("The request is invalid.", "request");
}
if (_currentRequest != null)
{
throw new InvalidOperationException("A new request cannot be started while another request is being processed.");
}
_currentRequest = request;
var start = request.fromGrid.GetCell(request.from, false) as IPathNode;
_goal = request.toGrid.GetCell(request.to, false) as IPathNode;
if (start == null)
{
CompleteRequest(PathingStatus.StartOutsideGrid);
return(false);
}
if (_goal == null)
{
if (_currentRequest.pathFinderOptions.navigateToNearestIfBlocked)
{
_goal = request.toGrid.GetCell(request.to, true) as IPathNode;
request.to = _goal.position;
}
else
{
CompleteRequest(PathingStatus.EndOutsideGrid);
return(false);
}
}
if (!_goal.isWalkable(_currentRequest.requesterProperties.attributes) && !_currentRequest.pathFinderOptions.navigateToNearestIfBlocked)
{
CompleteRequest(PathingStatus.DestinationBlocked);
return(false);
}
OnStart(start);
return(true);
}
void INeedPath.ConsumePathResult(PathResult result)
{
// Execute on the main thread to avoid multi-threading issues or the need for using a lock or Monitor
NavLoadBalancer.marshaller.ExecuteOnMainThread(() =>
{
// If we have stopped or get back the result of a request other than the one we currently expect, just toss it as it will be outdated.
if (result.originalRequest != _pendingPathRequest)
{
return;
}
_pendingResult = result;
_pendingPathRequest = null;
});
}
private void StopInternal()
{
lock (_syncLock)
{
_onFinalApproach = false;
_stopped = true;
_wayPoints.Clear();
_pathboundWayPoints.Clear();
_currentPath = null;
_pendingPathRequest = null;
_currentDestination = null;
_pendingResult = null;
_manualReplan = null;
}
}
public void Start(IPathRequest req)
{
_actualRequest = req;
_actualFrom = req.from;
if (req.via != null && req.via.Length > 0)
{
_viaIdx = 0;
_actualTo = req.via[0];
}
else
{
_viaIdx = -1;
_actualTo = req.to;
}
}
private IEnumerator ProcessRequestCoroutineInternal(IPathRequest request)
{
if (!StartRequest(request))
{
yield break;
}
var status = PathingStatus.Running;
while (status == PathingStatus.Running)
{
status = ProcessNext();
yield return(null);
}
CompleteRequest(status);
}
/// <summary>
/// Factory method to create a result with a path
/// </summary>
/// <param name="pathPoints">The path points.</param>
/// <param name="endWaypoint">The end way point.</param>
/// <param name="originalRequest">The original request.</param>
/// <returns>The result</returns>
public static DirectPathResult CreateWithPath(Vector3[] pathPoints, Vector3 endWaypoint, IPathRequest originalRequest)
{
var res = new DirectPathResult();
var path = new Path(pathPoints.Length);
for (int i = pathPoints.Length - 1; i >= 0; i--)
{
path.Push(new Position(pathPoints[i]));
}
res.path = path;
res.pendingWaypoints = new[] { endWaypoint };
res.originalRequest = originalRequest;
res.status = PathingStatus.Complete;
return res;
}
internal static InternalPathRequest Internalize(IPathRequest request)
{
var internalized = request as InternalPathRequest;
if (internalized == null)
{
internalized = new InternalPathRequest();
Utils.CopyProps(request, internalized);
}
internalized.pathType = Type.Normal;
return internalized;
}
private void StopInternal()
{
lock (_syncLock)
{
_stopped = true;
_wayPoints.Clear();
_currentPath = null;
_pendingPathRequest = null;
_currentDestination = null;
_pendingResult = null;
_manualReplan = null;
}
}
private void RequestPath(Vector3 from, Vector3 to, InternalPathRequest.Type type)
{
var unit = this.GetUnitFacade();
lock (_syncLock)
{
_pendingPathRequest = new InternalPathRequest
{
from = from,
to = to,
pathType = type,
requester = this,
requesterProperties = unit,
pathFinderOptions = unit.pathFinderOptions,
timeStamp = Time.time
};
if (type == InternalPathRequest.Type.Normal)
{
_pendingPathRequest.via = _wayPoints.GetViaPoints();
}
_stop = false;
_stopped = false;
}
GameServices.pathService.QueueRequest(_pendingPathRequest, unit.pathFinderOptions.pathingPriority);
}
/// <summary>
/// Requests the path.
/// </summary>
/// <param name="request">The request.</param>
public void RequestPath(IPathRequest request)
{
var unit = this.GetUnitFacade();
request = InternalPathRequest.Internalize(request);
request.timeStamp = Time.time;
lock (_syncLock)
{
request.requester = this;
request.requesterProperties = unit;
_pendingPathRequest = request;
_stop = false;
_stopped = false;
}
GameServices.pathService.QueueRequest(_pendingPathRequest, unit.pathFinderOptions.pathingPriority);
}
private void FixupGoal(IPathRequest request)
{
var unitProps = request.requesterProperties;
var requesterRadius = request.requesterProperties.radius;
var actualGoal = request.to;
var halfCell = _goal.parent.cellSize / 2.0f;
var dx = actualGoal.x - _goal.position.x;
var dz = actualGoal.z - _goal.position.z;
var overlapLeft = (requesterRadius - dx) - halfCell;
var overlapRight = (dx + requesterRadius) - halfCell;
var overlapTop = (dz + requesterRadius) - halfCell;
var overlapBottom = (requesterRadius - dz) - halfCell;
var adjX = 0.0f;
var adjZ = 0.0f;
if (overlapLeft > 0.0f && !ValidateGoalNeighbour(unitProps, -1, 0))
{
adjX = -overlapLeft;
}
else if (overlapRight > 0.0f && !ValidateGoalNeighbour(unitProps, 1, 0))
{
adjX = overlapRight;
}
if (overlapTop > 0.0f && !ValidateGoalNeighbour(unitProps, 0, 1))
{
adjZ = overlapTop;
}
else if (overlapBottom > 0.0f && !ValidateGoalNeighbour(unitProps, 0, -1))
{
adjZ = -overlapBottom;
}
//If we do overlap direct neighbours but they are free we have to ensure that the diagonal is also free.
if ((adjX == 0.0f) && (adjZ == 0.0f))
{
if ((overlapLeft > 0.0f) && (overlapTop > 0.0f) && !ValidateGoalNeighbour(unitProps, -1, 1))
{
adjX = -overlapLeft;
adjZ = overlapTop;
}
else if ((overlapLeft > 0.0f) && (overlapBottom > 0.0f) && !ValidateGoalNeighbour(unitProps, -1, -1))
{
adjX = -overlapLeft;
adjZ = -overlapBottom;
}
else if ((overlapRight > 0.0f) && (overlapTop > 0.0f) && !ValidateGoalNeighbour(unitProps, 1, 1))
{
adjX = overlapRight;
adjZ = overlapTop;
}
else if ((overlapRight > 0.0f) && (overlapBottom > 0.0f) && !ValidateGoalNeighbour(unitProps, 1, -1))
{
adjX = overlapRight;
adjZ = -overlapBottom;
}
}
if ((adjX != 0.0f) || (adjZ != 0.0f))
{
request.to = new Vector3(actualGoal.x - adjX, actualGoal.y, actualGoal.z - adjZ);
}
}
private void CompleteRequest(PathingStatus status)
{
PathResult result;
if (_currentRequest.type == RequestType.IntelOnly)
{
result = new PathResult(status, null, _goal.g, _currentRequest);
}
else if (status == PathingStatus.Complete)
{
Path path;
var maxPathLength = Mathf.CeilToInt(_goal.g / (_goal.parent.cellSize * _costProvider.baseMoveCost));
//Fix the actual destination so it does not overlap obstructions
FixupGoal(_currentRequest);
if (_currentRequest.pathFinderOptions.usePathSmoothing)
{
path = _smoother.Smooth(_goal, maxPathLength, _currentRequest, _cellCostStrategy);
}
else
{
path = new Path(maxPathLength);
//Push the actual end position as the goal
path.Push(new Position(_currentRequest.to));
IPathNode current = _goal.predecessor;
while (current != null)
{
path.Push(current);
current = current.predecessor;
}
//Instead of testing for it in the while loop, just pop off the start node and replace it with the actual start position
if (path.count > 1)
{
path.Pop();
}
path.Push(new Position(_currentRequest.from));
}
result = new PathResult(status, path, _goal.g, _currentRequest);
}
else
{
result = new PathResult(status, null, 0, _currentRequest);
}
_currentRequest.Complete(result);
_currentRequest = null;
}
void INeedPath.ConsumePathResult(PathResult result)
{
// Execute on the main thread to avoid multi-threading issues or the need for using a lock or Monitor
NavLoadBalancer.marshaller.ExecuteOnMainThread(() =>
{
// If we have stopped or get back the result of a request other than the one we currently expect, just toss it as it will be outdated.
if (result.originalRequest != _pendingPathRequest)
{
return;
}
_pendingResult = result;
_pendingPathRequest = null;
});
}
/// <summary>
/// Resolves the direct path or delegates the request on to path finding.
/// </summary>
/// <param name="req">The request.</param>
/// <returns>A path request to use in path finding or null if the path was resolved.</returns>
public IPathRequest ResolveDirectPath(IPathRequest req)
{
var to = req.to;
var from = req.from;
var toGrid = req.toGrid;
var fromGrid = req.fromGrid;
var unitMask = req.requester.attributes;
//If no grids were resolved for this request it means the request involves two points outside the grid(s) that do not cross any grid(s), so we can move directly between them
if (fromGrid == null && toGrid == null)
{
req.Complete(new DirectPathResult(from, to, req));
return null;
}
//Is the start node on a grid
Cell fromCell = null;
if (fromGrid != null)
{
fromCell = fromGrid.GetCell(from);
}
//If the from cell is blocked, we have to get out to a free cell first
if (fromCell != null && !fromCell.isWalkable(unitMask))
{
//Check for a free cell to escape to before resuming the planned path. If no free cell is found we are stuck.
//The unit may still get stuck even if a free cell is found, in case there is a physically impassable blockade
var escapeCell = fromGrid.GetNearestWalkableCell(from, from, true, req.maxEscapeCellDistanceIfOriginBlocked, unitMask);
if (escapeCell == null)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return null;
}
//Move to the free cell and then on with the planned path
req.Complete(DirectPathResult.CreateWithPath(from, escapeCell.position, to, req));
return null;
}
//Is the destination node on a grid
Cell toCell = null;
if (toGrid != null)
{
toCell = toGrid.GetCell(to);
}
//If the destination is blocked then...
if (toCell != null && !toCell.isWalkable(unitMask))
{
req.Complete(DirectPathResult.Fail(PathingStatus.DestinationBlocked, req));
return null;
}
//If both cells have been resolved, we need a final check to ensure that they are either on the same grid or that a portal exists
bool crossGrid = (fromGrid != toGrid);
if (fromCell != null && toCell != null)
{
if (!crossGrid || req.preventOffGridNavigation || GridManager.instance.PortalExists(fromGrid, toGrid, unitMask))
{
return req;
}
}
else if (req.preventOffGridNavigation)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return null;
}
//We have now determined that no path can be found using standard pathfinding, so now we find a path by way of off-grid navigation.
//Find the perimeters that are in play. There can at most be two. The reference is 'from' unless 'from' is inside the grid, then its 'to'
//The positions are considered to be both outside, when the from position is outside and either the destination is outside or the from position has an associated (crossed) grid that is different from the destination grid.
bool bothOutside = (fromCell == null) && (toCell == null || ((fromGrid != null) && crossGrid));
var reference = (fromCell == null) ? from : to;
IGrid grid = null;
if (fromGrid == null)
{
grid = toGrid;
}
else
{
grid = fromGrid;
req.toGrid = fromGrid;
}
_crossedPerimeters.Clear();
Vector3 crossingPoint;
if (reference.x < grid.left.edge)
{
if (CheckEdgeX(from, to, grid.left, grid.bottom, grid.top, out crossingPoint))
{
var pc = new PerimeterCell(grid.left);
pc.cell = grid.GetNearestWalkablePerimeterCell(crossingPoint, Vector3.left, unitMask, true);
_crossedPerimeters.Add(pc);
}
}
else if (reference.x > grid.right.edge)
{
if (CheckEdgeX(from, to, grid.right, grid.bottom, grid.top, out crossingPoint))
{
var pc = new PerimeterCell(grid.right);
pc.cell = grid.GetNearestWalkablePerimeterCell(crossingPoint, Vector3.right, unitMask, true);
_crossedPerimeters.Add(pc);
}
}
if (reference.z < grid.bottom.edge)
{
if (CheckEdgeZ(from, to, grid.bottom, grid.left, grid.right, out crossingPoint))
{
var pc = new PerimeterCell(grid.bottom);
pc.cell = grid.GetNearestWalkablePerimeterCell(crossingPoint, Vector3.back, unitMask, true);
_crossedPerimeters.Add(pc);
}
}
else if (reference.z > grid.top.edge)
{
if (CheckEdgeZ(from, to, grid.top, grid.left, grid.right, out crossingPoint))
{
var pc = new PerimeterCell(grid.top);
pc.cell = grid.GetNearestWalkablePerimeterCell(crossingPoint, Vector3.forward, unitMask, true);
_crossedPerimeters.Add(pc);
}
}
//If no actual boundary crossings exist, it means both are outside the grid and a direct route between them, that will not cross the grid exists.
if (_crossedPerimeters.Count == 0)
{
req.Complete(new DirectPathResult(from, to, req));
return null;
}
//if from and to are both outside the grid, but the path between them does cross a perimeter.
//No direct path exist so just go to the corner nearest to 'to' and try again, since there is a path.
float shortestDistance = float.MaxValue;
if (bothOutside)
{
Vector3 closestCorner = Vector3.zero;
for (int i = 0; i < _crossedPerimeters.Count; i++)
{
var p = _crossedPerimeters[i].perimeter;
var d1 = (to - p.outsideCornerOne).sqrMagnitude;
var d2 = (to - p.outsideCornerTwo).sqrMagnitude;
if (d1 < shortestDistance)
{
shortestDistance = d1;
closestCorner = p.outsideCornerOne;
}
if (d2 < shortestDistance)
{
shortestDistance = d2;
closestCorner = p.outsideCornerTwo;
}
}
req.Complete(DirectPathResult.CreateWithPath(from, closestCorner, to, req));
return null;
}
//Find the closest edge cell
IGridCell edgeCell = null;
Perimeter targetPerimeter = null;
for (int i = 0; i < _crossedPerimeters.Count; i++)
{
var p = _crossedPerimeters[i];
if (p.cell == null)
{
continue;
}
var d = (from - p.cell.position).sqrMagnitude + (to - p.cell.position).sqrMagnitude;
if (d < shortestDistance)
{
shortestDistance = d;
edgeCell = p.cell;
targetPerimeter = p.perimeter;
}
}
Vector3 firstOutsidePoint;
//At least one actual boundary crossing was found, but no walkable cells were found on that boundary.
//Now we could check perpendiculars all (at most 2) perimeters to be sure to get the closest route, but...
if (edgeCell == null)
{
targetPerimeter = _crossedPerimeters[0].perimeter;
//Check the two perpendicular perimeters
var cellOne = grid.GetNearestWalkablePerimeterCell(targetPerimeter.insideCornerOne, targetPerimeter.perpendicularOne.edgeVector, unitMask, false);
var cellTwo = grid.GetNearestWalkablePerimeterCell(targetPerimeter.insideCornerTwo, targetPerimeter.perpendicularTwo.edgeVector, unitMask, false);
//If both have a point of entry, choose the one resulting in the shortest path
if (cellOne != null && cellTwo != null)
{
var dOne = (from - targetPerimeter.outsideCornerOne).sqrMagnitude + (to - targetPerimeter.outsideCornerOne).sqrMagnitude + (cellOne.position - targetPerimeter.outsideCornerOne).sqrMagnitude;
var dTwo = (from - targetPerimeter.outsideCornerTwo).sqrMagnitude + (to - targetPerimeter.outsideCornerTwo).sqrMagnitude + (cellTwo.position - targetPerimeter.outsideCornerTwo).sqrMagnitude;
if (dOne < dTwo)
{
cellTwo = null;
}
else
{
cellOne = null;
}
}
if (cellOne != null)
{
//If moving from outside the grid to the inside, just move to the corner
if (fromCell == null)
{
req.Complete(DirectPathResult.CreateWithPath(from, targetPerimeter.outsideCornerOne, to, req));
return null;
}
//Otherwise request a path to the edgeCell with a way point just outside + the destination
firstOutsidePoint = cellOne.position + (targetPerimeter.perpendicularOne.edgeVector * grid.cellSize);
return UpdateRequest(req, cellOne, firstOutsidePoint, to);
}
else if (cellTwo != null)
{
//If moving from outside the grid to the inside, just move to the corner
if (fromCell == null)
{
req.Complete(DirectPathResult.CreateWithPath(from, targetPerimeter.outsideCornerTwo, to, req));
return null;
}
//Otherwise request a path to the edgeCell with a way point just outside + the destination
firstOutsidePoint = cellTwo.position + (targetPerimeter.perpendicularTwo.edgeVector * grid.cellSize);
return UpdateRequest(req, cellTwo, firstOutsidePoint, to);
}
else
{
//Check the opposing side
var opposing = targetPerimeter.perpendicularOne.perpendicularOne;
edgeCell = grid.GetNearestWalkablePerimeterCell(opposing.GetPoint(to), opposing.edgeVector, unitMask, true);
if (edgeCell == null)
{
req.Complete(DirectPathResult.Fail(PathingStatus.NoRouteExists, req));
return null;
}
//If moving from outside the grid to the inside, still just move to corner from which the shortest route will go
if (fromCell == null)
{
var c1 = (from - targetPerimeter.outsideCornerOne).sqrMagnitude + (edgeCell.position - targetPerimeter.outsideCornerOne).sqrMagnitude;
var c2 = (from - targetPerimeter.outsideCornerTwo).sqrMagnitude + (edgeCell.position - targetPerimeter.outsideCornerTwo).sqrMagnitude;
var corner = (c1 < c2) ? targetPerimeter.outsideCornerOne : targetPerimeter.outsideCornerTwo;
req.Complete(DirectPathResult.CreateWithPath(from, corner, to, req));
return null;
}
//Otherwise request a path to the edgeCell with a way point just outside + the destination
firstOutsidePoint = edgeCell.position + (opposing.edgeVector * grid.cellSize);
return UpdateRequest(req, edgeCell, firstOutsidePoint, to);
}
}
//We found a valid perimeter crossing point, so create the path
firstOutsidePoint = edgeCell.position + (targetPerimeter.edgeVector * grid.cellSize);
//outside in
if (fromCell == null)
{
if (from == firstOutsidePoint)
{
req.Complete(DirectPathResult.CreateWithPath(from, edgeCell.position, to, req));
}
else
{
req.Complete(DirectPathResult.CreateWithPath(new Vector3[] { from, firstOutsidePoint, edgeCell.position }, to, req));
}
return null;
}
//If we are currently on the grid, first see if we are at the suggested edge cell, if so we can walk straight to the destination outside
if (edgeCell == fromCell)
{
if (firstOutsidePoint == to)
{
req.Complete(new DirectPathResult(from, to, req));
return null;
}
//While it would be possible to make a complete path here, we want to use a way point since 'to' could be inside another grid, so we need to do another request there
req.Complete(DirectPathResult.CreateWithPath(new Vector3[] { from, firstOutsidePoint }, to, req));
return null;
}
//inside out
return UpdateRequest(req, edgeCell, firstOutsidePoint, to);
}
/// <summary>
/// Queues a request with a priority. Higher values take priority.
/// </summary>
/// <param name="request">The request.</param>
/// <param name="priority">The priority.</param>
public void QueueRequest(IPathRequest request, int priority)
{
Ensure.ArgumentNotNull(request, "request");
lock (_queue)
{
_queue.Enqueue(request, priority);
if (this.runAsync && !_processingActive)
{
StartAsyncProcessing();
}
}
}
/// <summary>
/// Processes the request as a coroutine.
/// </summary>
/// <param name="request">The request.</param>
/// <returns>The coroutine enumerator</returns>
public IEnumerator ProcessRequestCoroutine(IPathRequest request)
{
_coroutineIter.Init(request);
return _coroutineIter;
}
private bool StartRequest(IPathRequest request)
{
Ensure.ArgumentNotNull(request, "request");
if (!request.isValid)
{
throw new ArgumentException("The request is invalid.", "request");
}
if (_currentRequest != null)
{
throw new InvalidOperationException("A new request cannot be started while another request is being processed.");
}
_currentRequest = request;
var start = request.fromGrid.GetCell(request.from, false) as IPathNode;
_goal = request.toGrid.GetCell(request.to, false) as IPathNode;
if (start == null)
{
CompleteRequest(PathingStatus.StartOutsideGrid);
return false;
}
if (_goal == null)
{
if (_currentRequest.pathFinderOptions.navigateToNearestIfBlocked)
{
_goal = request.toGrid.GetCell(request.to, true) as IPathNode;
request.to = _goal.position;
}
else
{
CompleteRequest(PathingStatus.EndOutsideGrid);
return false;
}
}
if (!_goal.isWalkable(_currentRequest.requesterProperties.attributes) && !_currentRequest.pathFinderOptions.navigateToNearestIfBlocked)
{
CompleteRequest(PathingStatus.DestinationBlocked);
return false;
}
OnStart(start);
return true;
}
public IEnumerator ProcessRequestCoroutineInternal(IPathRequest request)
{
//Granted this could be done smarter, but since this is not an expected scenario, we just use exception handling to handle it.
ValidateRequest(request);
_currentResult = new PathResult(PathingStatus.Complete, null, 0, request);
var status = PathingStatus.Complete;
//Iterate over the waypoints of the request, including the end point
_segmentRequest.Start(request);
do
{
status = StartPathSegment();
if (status != PathingStatus.Running)
{
break;
}
while (status == PathingStatus.Running)
{
status = ProcessNext();
yield return null;
}
if (!CompletePathSegment(status))
{
break;
}
}
while (_segmentRequest.MoveNext());
if (status != PathingStatus.Complete)
{
if (_segments.count > 0)
{
_currentResult.RegisterPartialResult(status, _segmentRequest.GetPendingWaypoints());
}
else
{
_currentResult.status = status;
}
}
//Splice the path segments and complete the request
_currentResult.path = Path.FromSegments(_segments);
request.Complete(_currentResult);
_segments.Clear();
_segmentRequest.Clear();
_currentResult = null;
}
private void ValidateRequest(IPathRequest request)
{
Ensure.ArgumentNotNull(request, "request");
if (!request.isValid)
{
throw new ArgumentException("The request is invalid.", "request");
}
if (_segmentRequest.isRunning)
{
throw new InvalidOperationException("A new request cannot be started while another request is being processed.");
}
}
private void RequestPath(Vector3 to, InternalPathRequest.Type type)
{
if (this.count == 0)
{
// if the group has no members, then no need to request anything
return;
}
// find a valid from position for the group
Vector3 fromPos = _modelUnit.position;
var grid = GridManager.instance.GetGrid(fromPos);
Cell fromCell = grid.GetCell(fromPos, true);
if (fromCell == null || !fromCell.isWalkable(_modelUnit.attributes))
{
fromCell = grid.GetNearestWalkableCell(fromPos, fromPos, false, _modelUnit.pathFinderOptions.maxEscapeCellDistanceIfOriginBlocked, _modelUnit);
if (fromCell != null)
{
fromPos = fromCell.position;
}
}
// setup the path request
_pendingPathRequest = new InternalPathRequest
{
from = fromPos,
to = to,
pathType = type,
requester = this,
requesterProperties = _modelUnit,
pathFinderOptions = _modelUnit.pathFinderOptions
};
_stopped = false;
_lastPathRequestTime = Time.time;
GameServices.pathService.QueueRequest(_pendingPathRequest, _modelUnit.pathFinderOptions.pathingPriority);
}
private static IPathRequest UpdateRequest(IPathRequest req, IGridCell to, params Vector3[] waypoints)
{
req.to = to.position;
req.pendingWaypoints = waypoints;
return req;
}
/// <summary>
/// Requests a path.
/// </summary>
/// <param name="request">The request.</param>
public void RequestPath(IPathRequest request)
{
if (this.count == 0)
{
// if the group has no members, then no need to request anything
return;
}
request = InternalPathRequest.Internalize(request);
request.requester = this;
request.requesterProperties = _modelUnit;
_pendingPathRequest = request;
_stopped = false;
_lastPathRequestTime = Time.time;
GameServices.pathService.QueueRequest(_pendingPathRequest, _modelUnit.pathFinderOptions.pathingPriority);
}
/// <summary>
/// Processes the request.
/// </summary>
/// <param name="request">The request.</param>
public void ProcessRequest(IPathRequest request)
{
try
{
if (!StartRequest(request))
{
return;
}
var status = PathingStatus.Running;
while (status == PathingStatus.Running)
{
status = ProcessNext();
}
CompleteRequest(status);
}
catch (Exception e)
{
Debug.LogWarning("Exception " + e);
FailRequest(request, e);
}
}
//Note this may be called from another thread if the PathService is running asynchronously
void INeedPath.ConsumePathResult(PathResult result)
{
lock (_syncLock)
{
//If we have stooped or get back the result of a request other than the one we currently expect, just toss it as it will be outdated.
if (_stopped || result.originalRequest != _pendingPathRequest)
{
return;
}
_pendingResult = result;
_pendingPathRequest = null;
}
}
public void Start(IPathRequest req)
{
_actualRequest = req;
_actualFrom = req.from;
if (req.via != null && req.via.Length > 0)
{
_viaIdx = 0;
_actualTo = req.via[0];
}
else
{
_viaIdx = -1;
_actualTo = req.to;
}
}
private void FailRequest(IPathRequest request, Exception e)
{
_segments.Clear();
_segmentRequest.Clear();
_currentResult = null;
var result = new PathResult(PathingStatus.Failed, null, 0, request)
{
errorInfo = string.Concat(e.Message, Environment.NewLine, e.StackTrace)
};
request.Complete(result);
}
public void Clear()
{
_actualRequest = null;
}
public void Init(IPathRequest request)
{
_request = request;
_innerIter = _engine.ProcessRequestCoroutineInternal(request);
}
/// <summary>
/// Queues a request.
/// </summary>
/// <param name="request">The request.</param>
public void QueueRequest(IPathRequest request)
{
QueueRequest(request, 0);
}
private IEnumerator ProcessRequestCoroutineInternal(IPathRequest request)
{
if (!StartRequest(request))
{
yield break;
}
var status = PathingStatus.Running;
while (status == PathingStatus.Running)
{
status = ProcessNext();
yield return null;
}
CompleteRequest(status);
}
