/// <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 StackWithLookAhead<IPositioned>(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;
}
private void StopInternal()
{
lock (_syncLock)
{
_stopped = true;
_wayPoints.Clear();
_pathboundWayPoints.Clear();
_currentPath = null;
_pendingPathRequest = null;
_currentDestination = null;
_pendingResult = null;
_manualPath = null;
}
}
private void CompleteRequest(PathingStatus status)
{
if (status == PathingStatus.Complete)
{
StackWithLookAhead<IPositioned> 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.usePathSmoothing)
{
path = _smoother.Smooth(_goal, maxPathLength, _currentRequest);
}
else
{
path = new StackWithLookAhead<IPositioned>(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));
}
_currentRequest.Complete(status, path);
}
else
{
_currentRequest.Complete(status, null);
}
_currentRequest = null;
}
private void SetManualPath(ManualPath path)
{
if (path.path.count == 0)
{
StopInternal();
return;
}
_stop = false;
_stopped = false;
_currentDestination = null;
_currentPath = path.path;
_currentGrid = GridManager.instance.GetGrid(_currentPath.Peek().position);
_endOfResolvedPath = _currentPath.Last().position;
_endOfPath = _endOfResolvedPath;
_lastPathRequestTime = Time.time;
}
private void ConsumeResult()
{
//Consume way points if appropriate. This must be done prior to the processing of the result, since if the request was a way point request, the first item in line is the one the result concerns.
if (_pendingResult.originalRequest.type == RequestType.Waypoint)
{
_wayPoints.Dequeue();
}
else if (_pendingResult.originalRequest.type == RequestType.PathboundWaypoint)
{
_pathboundWayPoints.Dequeue();
}
//Reset current destination no matter what
_currentDestination = null;
//Since result processing may actually repath and consequently a new result may arrive we need to operate on locals and null the pending result
var result = _pendingResult;
_pendingResult = null;
//Process the result
if (!ProcessAndValidateResult(result))
{
return;
}
//Consume the result
_currentPath = result.path;
_currentGrid = result.originalRequest.fromGrid;
_endOfResolvedPath = _currentPath.Last().position;
_endOfPath = _endOfResolvedPath;
//Update pending way points
UpdatePathboundWaypoints(result.pendingWaypoints);
//The first point on the path is always the origin of the request, so we want to skip that.
_currentPath.Pop();
}
/// <summary>
/// Gets the movement vector.
/// </summary>
/// <param name="currentVelocity">The current velocity.</param>
/// <returns>
/// The movement vector
/// </returns>
protected override Vector3 GetMovementVector(Vector3 currentVelocity)
{
if (_stopped || _wait)
{
return Vector3.zero;
}
if (_stop)
{
StopInternal();
return Vector3.zero;
}
//If we have no path but a result is ready, get the path from the result
if (_currentPath == null && _pendingResult != null)
{
ConsumeResult();
}
if (_currentDestination == null)
{
//If a request exists but we haven't yet gotten a route, do local pathing until we get one
if ((_currentPath == null) || (_currentPath.count == 0))
{
//need a local var here to ensure thread safety
var pending = _pendingPathRequest;
if (pending != null)
{
return SteerLocally(pending.to);
}
return Vector3.zero;
}
//Get the next destination from the path
ResolveNextPoint();
}
//Get the direction
var curDir = ResolveMoveDirection();
HandleWaypointsAndArrival(curDir);
if (_currentDestinationDistance < this.nextNodeDistance)
{
//We are inside the bounds of a route node, so transition to the next node or move towards arrival
if (_currentPath != null && _currentPath.count > 0)
{
if (this.announceAllNodes)
{
AnnounceEvent(UnitNavigationEventMessage.Event.NodeReached, _currentDestination.position, null);
}
ResolveNextPoint();
return GetMovementVector(currentVelocity);
}
else if (_currentDestinationDistance < this.arrivalDistance)
{
if (_pendingResult != null)
{
//A pending route exists (i.e. next way point) so move on to that one right away
if (_pendingResult.originalRequest.type != RequestType.PathboundWaypoint)
{
AnnounceEvent(UnitNavigationEventMessage.Event.WaypointReached, _currentDestination.position, null);
}
_currentPath = null;
return GetMovementVector(currentVelocity);
}
if (_pendingPathRequest != null)
{
//We have reach the end of the current path but have a pending request, so we have to basically stand still and wait for it. Obviously this will only happen if requests take a long time to complete.
return Vector3.zero;
}
//We are within the arrival distance on the end node so stop
StopInternal();
AnnounceEvent(UnitNavigationEventMessage.Event.DestinationReached, this.transformCached.position, null);
return Vector3.zero;
}
}
else
{
HandlePathReplan();
}
return curDir;
}
/// <summary>
/// Replans the path.
/// </summary>
public void ReplanPath()
{
if (_stopped || _pendingPathRequest != null)
{
return;
}
_currentPath = null;
_pathboundWayPoints.Clear();
if (_manualPath != null && _manualPath.onReplan != null)
{
_manualPath.onReplan(this.gameObject, _endOfPath, _manualPath);
SetManualPath(_manualPath);
}
else
{
RequestPath(this.transformCached.position, _endOfPath, RequestType.Normal);
}
}
/// <summary>
/// Smooths a path.
/// </summary>
/// <param name="goal">The goal node of the calculated path.</param>
/// <param name="maxPathLength">Maximum length of the path.</param>
/// <param name="request">The path request.</param>
/// <returns>
/// The path in smoothed form
/// </returns>
public StackWithLookAhead<IPositioned> Smooth(IPathNode goal, int maxPathLength, IPathRequest request)
{
var requesterAttributes = request.requester.attributes;
var requesterRadius = request.requester.radius;
//Next prune superfluous path nodes
var reversePath = new List<IPositioned>(maxPathLength);
var current = goal;
var next = current.predecessor;
int bends = -1;
var prevDir = Vector3.zero;
while (next != null)
{
var dir = next.position - current.position;
if ((dir != prevDir) || (next is IPortalNode))
{
reversePath.Add(current);
prevDir = dir;
bends++;
}
current = next;
next = current.predecessor;
}
//Correct the end nodes and inject a mid point if too much was pruned (can happen on straight paths with no direction change, which can lead to obstacle collision if the unit is offset)
if (reversePath.Count == 0)
{
reversePath.Add(new Position(request.to));
}
else
{
reversePath[0] = new Position(request.to);
}
if (reversePath.Count == 1 && bends <= 0)
{
reversePath.Add(goal.predecessor);
}
reversePath.Add(new Position(request.from));
//Next see if we can reduce the path further by excluding unnecessary bends
if (!request.preventDiagonalMoves)
{
var matrix = goal.parent;
for (int i = 0; i < reversePath.Count - 2; i++)
{
var c1 = reversePath[i];
var c2 = reversePath[i + 1];
var c3 = reversePath[i + 2];
var skip = AdjustIfPortal(c1, c2, c3);
if (skip > -1)
{
//One of the candidate nodes is a portal so skip to the node following the portal and resolve the grid at the other end of the portal.
//Since a portal node will never be the last node we can safely do this here. Since we are moving in the reverse direction here the portal will be on the other side.
i += skip;
matrix = ((IPortalNode)reversePath[i]).parent;
continue;
}
if (CanReducePath(c1, c3, requesterAttributes, requesterRadius, matrix))
{
reversePath[i + 1] = null;
i++;
}
}
}
//Construct the final path replacing the start and goal nodes with the actual start and goal positions
var path = new StackWithLookAhead<IPositioned>();
for (int i = 0; i < reversePath.Count; i++)
{
var node = reversePath[i];
if (node != null)
{
path.Push(node);
}
}
return path;
}