private IEnumerator FollowPlayer()
{
while (true)
{
yield return(waitForNewPath);
if (playerTransform == null)
{
playerTransform = GameObject.FindGameObjectWithTag("Player").GetComponent <Transform>();
}
closeToPlayerNodes = Physics.OverlapSphere(playerTransform.position, inaccuracy, walkableMask);
int randomNode = UnityEngine.Random.Range(1, closeToPlayerNodes.Length - 1);
try
{
pathRequest = new PathRequestData(thisTransform.position, closeToPlayerNodes[randomNode].transform.position, PathSubmitted);
}
catch
{
print("<color=blue>Path request data can't be set up!</color>");
}
try
{
pathManager.RequestPath(pathRequest);
}
catch
{
print("<color=blue>Path request failed!</color>");
}
}
}
// Request a new path to start on a new thread
public static void RequestThreadPath(PathRequestData request)
{
// Starts this delegate on a new a thread
ThreadStart threadStart = delegate
{
// From the main instance (Singleton instance), call the path-finding method
_instance._pathFindingManager.FindPath(request, _instance.FinishedProcessingThreadPath);
};
threadStart.Invoke(); // Invoke the thread
}
public void RequestPath(PathRequestData pathRequest)
{
pathfinding.FindPathFromAtoB(pathRequest);
}
public void FindPath(PathRequestData requestData, Action <PathResultData> callbackAction)
{
Stopwatch sw = new Stopwatch(); /* Used in order to see the performance of the path-finding system before/after heap tree optimization.
* Stopwatch creates some kind of overhead so disable it in the final build.*/
sw.Start();
// CallbackPathResult elements
Vector2[] pathWayPoints = new Vector2[0];
bool pathSuccess = false;
#region A* Algorithm
// Find the start and end nodes on the current grid system
Node startNode = _gridSystem.NodeFromWorldPoint(requestData.PathStart);
Node targetNode = _gridSystem.NodeFromWorldPoint(requestData.PathEnd);
// Only do calculations if nodes are walkable
if (/*startNode.Walkable &&*/ targetNode.Walkable)
{
HeapTree <Node> openNodeSet = new HeapTree <Node>(_gridSystem.MaxHeapSize); // Binary Heap Tree for A* iteration - optimization
HashSet <Node> closedNodeSet = new HashSet <Node>(); // Hash Set for the closed node set of the A* algorithm
//
openNodeSet.AddItem(startNode); // Begin with the starting node
while (openNodeSet.HeapTreeSize > 0) // While the open node set is not empty
{
Node currentNode = openNodeSet.RemoveFirst(); // First in heap tree is the one with the lowest score.
closedNodeSet.Add(currentNode); // Transfer this node from open set to closed set
if (currentNode == targetNode) // PathResult status check - if the target node is reached
{
sw.Stop();
print("Path found in : " + sw.ElapsedMilliseconds + " mili-seconds ");
pathSuccess = true;
break; // PathResult has been found. Exit the while loop
} // If not found start neighbor cost calculations
// List of the adjacent neighboring nodes. They are pre-calculated and stored in Nodes - optimization (Heap -> Stack Memory Allocation)
List <Node> neighborNodeSet = currentNode.NeighborNodes; // Get the pre-computed neighbor nodes
for (int i = 0; i < neighborNodeSet.Count; i++)
{
if (!neighborNodeSet[i].Walkable || closedNodeSet.Contains(neighborNodeSet[i]))
{
continue; // Non-walkable nodes and already closed nodes are passed
}
// New movement G cost with the path through current node
int movementCostToNeighbor = currentNode.GCost + GetDistance(currentNode, neighborNodeSet[i]);
// If the new distance from starting node is shorter and open node set does not contain this neighbor
if (movementCostToNeighbor < neighborNodeSet[i].GCost || !openNodeSet.ContainsItem(neighborNodeSet[i]))
{
neighborNodeSet[i].GCost = movementCostToNeighbor; // Update the distance from starting node with its new path
neighborNodeSet[i].HCost = GetDistance(neighborNodeSet[i], targetNode);
neighborNodeSet[i].ParentNode = currentNode; // Set the new parent of the node for final path detection
if (!openNodeSet.ContainsItem(neighborNodeSet[i])) // If it is not in the open set, add it to the set
{
openNodeSet.AddItem(neighborNodeSet[i]);
}
else
{
openNodeSet.UpdateItem(neighborNodeSet[i]); // If already in the set then the values are changed
}
}
}
}
}
#endregion
// If an available path is found
if (pathSuccess)
{
pathWayPoints = ReTracePath(startNode, targetNode); // Find the path way-points by re-tracing parent nodes
pathSuccess = pathWayPoints.Length > 0; // Set path to success if there is at least one way-point
}
// Trigger the callback, path is found. This will trigger the queue in the path-finding request manager
callbackAction(new PathResultData(pathWayPoints, _wayPointNodesCache, pathSuccess, requestData.CallbackPathRequest));
}
public void Update(int maxIter = 100)
{
var s = state[0];
PathQueryStatus status = PathQueryStatus.Success;
if (s.entitySize == 0)
{
return;
}
while (maxIter > 0)
{
if (s.entityInUse == State.NONE)
{
PathRequestData request = requests[0];
for (int i = s.entitySize - 1; i >= 0; --i)
{
request = requests[i];
if (request.status < PathRequestStatus.Done)
{
s.entityInUse = i;
break;
}
}
if (s.entityInUse == State.NONE)
{
break;
}
var startLoc = query.MapLocation(request.start, Vector3.one * 10f, request.agentType, request.mask);
var endLoc = query.MapLocation(request.end, Vector3.one * 10f, request.agentType, request.mask);
status = query.BeginFindPath(startLoc, endLoc, request.mask);
if (status.IsFailure())
{
break;
}
request.status = PathRequestStatus.InProgress;
requests[s.entityInUse] = request;
}
int nIter;
status = query.UpdateFindPath(maxIter, out nIter);
if (status.IsFailure())
{
break;
}
maxIter -= nIter;
if (status.IsSuccess())
{
int pathSize;
var request = requests[s.entityInUse];
status = query.EndFindPath(out pathSize);
if (status.IsFailure())
{
break;
}
if (status.IsSuccess())
{
query.GetPathResult(resultBuffer);
var offset = s.pathSize;
var pathSlice = new NativeSlice <PathPoint>(pathBuffer, offset);
status = PathUtils.FindStraightPath(query, request.start, request.end, resultBuffer, pathSize
, pathSlice, ref pathSize, MAX_PATHSIZE);
if (status.IsFailure())
{
break;
}
pathStart[s.entityInUse] = offset;
request.pathSize = pathSize;
s.pathSize += pathSize;
}
request.status = PathRequestStatus.Done;
requests[s.entityInUse] = request;
s.entityInUse = State.NONE;
}
state[0] = s;
}
if (status.IsFailure())
{
var request = requests[s.entityInUse];
request.status = PathRequestStatus.Failure;
requests[s.entityInUse] = request;
s.entityInUse = State.NONE;
state[0] = s;
}
}
public void FindPathFromAtoB(PathRequestData pathRequest)
{
ASNode startNode = grid.GetNodeFromWorldPoint(pathRequest.startPos);
ASNode finishNode = grid.GetNodeFromWorldPoint(pathRequest.finishPos);
if (finishNode.walkable == false)
{
return;
}
List <ASNode> openNodes = new List <ASNode>();
HashSet <ASNode> closedNodes = new HashSet <ASNode>();
openNodes.Add(startNode);
while (openNodes.Count > 0)
{
ASNode currentNode = openNodes[0];
for (int i = 1; i < openNodes.Count; i++)
{
if (openNodes[i].FCost < currentNode.FCost || openNodes[i].FCost == currentNode.FCost && openNodes[i].hCost < currentNode.hCost)
{
if (openNodes[i].hCost < currentNode.hCost)
{
currentNode = openNodes[i];
}
}
}
openNodes.Remove(currentNode);
closedNodes.Add(currentNode);
if (currentNode == finishNode)
{
RetracePath(startNode, finishNode);
pathRequest.pathGetter(path);
return;
}
foreach (ASNode neighbour in grid.GetNodeNeighbors(currentNode))
{
if (!neighbour.walkable || closedNodes.Contains(neighbour))
{
continue;
}
int newGCostToNeighbour = currentNode.gCost + GetManhattanDistance(currentNode, neighbour);
if (newGCostToNeighbour < neighbour.gCost || !openNodes.Contains(neighbour))
{
neighbour.gCost = newGCostToNeighbour;
neighbour.hCost = GetManhattanDistance(neighbour, finishNode);
neighbour.parentNode = currentNode;
if (!openNodes.Contains(neighbour))
{
openNodes.Add(neighbour);
}
}
}
}
}
