/// <summary>
/// Called when the surface becomes available.
/// </summary>
/// <param name="delayedRequest">A request that was delayed until surface becomes available.</param>
protected override void OnSurfaceAvailable(PathfindingRequest delayedRequest)
{
if (currentFace == null ||
!AlchemyNavigationSystem.Current.ContainsFace(Layer, currentFace.source))
{
var ray = new Ray(position, Vector3.down);
if (AlchemyNavigationSystem.Current.Raycast(ray, Layer, AreaMask, out var result))
{
currentFace = new CachedFace(result.face);
position = result.position;
ApplyPosition();
}
else
{
Debug.LogWarning("Agent couldn't be placed on navigation surface.");
gameObject.SetActive(false);
return;
}
}
if (delayedRequest != null)
{
CancelAllRequests();
RequestPath(position, delayedRequest.endPosition, PathType, currentFace.source);
}
else if (IsPathWalking)
{
Vector3 destination = progress.pointPath[progress.pointsCount - 1];
CancelAllRequests();
RequestPath(position, destination, PathType, currentFace.source);
}
}
/**
* Runs a pathfinding test on a simple world with a ] shape.
*/
public bool RunPathfindingTest(bool validPath = true)
{
//Setup the world nodes
worldSize = 8;
worldNodes = new Node[worldSize, worldSize];
//Fill the world nodes array with default values
for (int x = 0; x < 8; x++)
{
for (int y = 0; y < 8; y++)
{
Node newNode = new Node(x, y);
newNode.SetBlocked(true);
worldNodes[x, y] = newNode;
}
}
worldNodes[1, 1].SetBlocked(false);
worldNodes[2, 1].SetBlocked(false);
worldNodes[3, 1].SetBlocked(false);
worldNodes[4, 1].SetBlocked(false);
worldNodes[4, 2].SetBlocked(false);
worldNodes[4, 3].SetBlocked(false);
worldNodes[4, 4].SetBlocked(!validPath);
worldNodes[4, 5].SetBlocked(false);
worldNodes[4, 6].SetBlocked(false);
worldNodes[3, 6].SetBlocked(false);
worldNodes[2, 6].SetBlocked(false);
Log.PrintDebug("Created simple world for testing.");
PathfindingRequest request = new PathfindingRequest();
request.start_x = 1;
request.start_y = 1;
request.end_x = 2;
request.end_y = 6;
return(CalculatePath(request));
}
private PathfindingRequest CreateNewRequest()
{
// Creates a new request object, with the destination being a random position on the map.
int endX;
int endY;
while (true)
{
endX = Random.Range(0, Map.Width);
endY = Random.Range(0, Map.Height);
// Is this end point inside a wall?
if (Map.Tiles[endX, endY] == true)
{
continue;
}
// Is this end point the same as the current position?
if (this.X == endX && this.Y == endY)
{
continue;
}
// Passed all the conditions, end the loop.
break;
}
// Create the request, with the callback method being UponPathCompleted.
var request = PathfindingRequest.Create(X, Y, endX, endY, UponPathCompleted, currentPath);
return(request);
}
private static void Run()
{
// Runs on the thread.
UnityEngine.Debug.Log("Running pathfinding multithread...");
while (run)
{
if (!writing)
{
if (pending.Count == 0)
{
Thread.Sleep(5);
continue;
}
PathfindingRequest request = pending.Dequeue();
if (request.IsValid())
{
watch.Reset();
watch.Start();
// Pathfind here.
List <Node> path = Pathfinding.Run(request.StartX, request.StartY, request.EndX, request.EndY, request.Layer);
acc++;
watch.Stop();
long elapsed = watch.ElapsedMilliseconds;
times.Add(elapsed);
if (times.Count > 100)
{
times.RemoveAt(0);
}
if (request.Done != null)
{
lock (hasPending)
{
hasPending.Remove(request.ID);
}
while (reading)
{
Thread.Sleep(10);
}
finished.Add(new KeyValuePair <UnityAction <List <Node> >, List <Node> >(request.Done, path));
}
else
{
lock (hasPending)
{
hasPending.Remove(request.ID);
}
UnityEngine.Debug.LogWarning("Wasted pathfinding (" + (path == null ? "NO PATH" : "PATH FOUND") + "), no receptor!");
}
}
}
}
UnityEngine.Debug.Log("Shutdown pathfinding multithread.");
}
public void Start()
{
// Initialize the current path list.
currentPath = new List <PNode>();
// Create the current request.
CurrentRequest = CreateNewRequest();
}
/// <summary>
/// Runs a pathfinding requests for a debug dummy path
/// </summary>
public void RequestDebugPath()
{
PathfindingRequest request = new PathfindingRequest();
request.start_x = 4;
request.start_y = 10;
request.end_x = 40;
request.end_y = 40;
CalculatePath(request);
}
public void PlotPath(Point destination)
{
if (IsPlottingPath)
{
CancelPathPlot();
}
var tp = base.TilePosition;
PathfindingRequest req = new PathfindingRequest(tp.X, tp.Y, destination.X, destination.Y, PathCallback);
realPathReq = JEngine.Pathfinding.Post(req);
}
/*public static int Closest(List<PathNode> inNodes, Vector3 toPoint)
* {
* int closestIndex = 0;
* float minDist = float.MaxValue;
* for(int i = 0; i < inNodes.Count; i++)
* {
* if(AStarHelper.Invalid(inNodes[i]))
* continue;
* float thisDist = Vector3.Distance(toPoint, inNodes[i].Position);
* if(thisDist > minDist)
* continue;
*
* minDist = thisDist;
* closestIndex = i;
* }
*
* if (minDist != float.MaxValue)
* return closestIndex;
* else
* return -1;
* }*/
/*public static List<PathNode> GetSources()
* {
* return sources;
* }*/
PathfindingRequest FindRequest(PathfindingRequest req)
{
foreach (PathfindingRequest pr in requests)
{
if (req.Id == pr.Id)
{
return(req);
}
}
return(null);
}
/// <summary>
/// Adds the surround nodes to a source node, and sets their conditions.
/// </summary>
private void AddSurroundingNodes(ref List <Node> toCheckList, Node source, PathfindingRequest request)
{
//Get the adjacent nodes
List <Node> adjacentNodes = GetAdjacentNodes(source);
//Check each node individually
//(secretly an embedded for loop which is a bit slow, but its only 4 long)
for (int i = 0; i < adjacentNodes.Count; i++)
{
Node current = adjacentNodes[i];
if (current.x == request.start_x && current.y == request.start_y)
{
continue;
}
//Ignore blocked nodes
if (current.GetBlocked())
{
continue;
}
//In this case the node next has a parent node.
//We need to check if by making source the parent node to current will make
//a quicker path
if (current.originNode != null && current.pathId == pathId)
{
//Our g cost will be the parent nodes gCost add 1
float newGCost = current.originNode.gCost + 1;
//Quicker to path through source
if (newGCost < current.gCost)
{
current.SetCosts(newGCost, current.hCost);
current.originNode = source;
}
}
//In this case current is independant and has no parent
//So we can add source as the parent instantly.
else
{
//We come from source
current.originNode = source;
//We are 1 further from source from the start
current.SetCosts(source.gCost + 1, QDistanceCalculation(current.x, current.y, request.end_x, request.end_y));
//We are updated for this path
current.pathId = pathId;
//We need to check this
if (!current.isChecked)
{
toCheckList.Add(current);
}
}
}
}
public static bool Find(string ID, int x, int y, int ex, int ey, TileLayer layer, UnityAction <List <Node> > done)
{
if (!run)
{
if (done != null)
{
done.Invoke(null);
}
return(false);
}
if (pending.Count >= MAX_PENDING)
{
if (done != null)
{
done.Invoke(null);
}
return(false);
}
if (hasPending.Contains(ID))
{
return(false);
}
PathfindingRequest r = new PathfindingRequest()
{
StartX = x, StartY = y, EndX = ex, EndY = ey, Layer = layer, Done = done, ID = ID
};
if (r.IsValid())
{
writing = true;
lock (hasPending)
{
hasPending.Add(ID);
}
pending.Enqueue(r);
writing = false;
return(true);
}
else
{
if (done != null)
{
done.Invoke(null);
}
return(false);
}
}
public List <PathNode> RequestPath(PathfindingRequest req, ref bool result)
{
List <PathNode> pathResult = null;
//float time1 = 0;
//float time2 = 0;
//DateTime dt1 = DateTime.Now;
//DateTime dt2 = DateTime.Now;
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
if (FindRequest(req) != null && FpsCounter.pathfindingsCounter <= maxPathFindingPerSecond)
{
FpsCounter.pathfindingsCounter++;
requests.Remove(req);
result = true;
sw.Start();
pathResult = GetPath(req.Start, req.End);
sw.Stop();
}
else if (requests.Count == 0 && FpsCounter.pathfindingsCounter <= maxPathFindingPerSecond)
{
FpsCounter.pathfindingsCounter++;
result = true;
sw.Start();
pathResult = GetPath(req.Start, req.End);
sw.Stop();
}
else
{
result = false;
requests.Add(req);
pathResult = null;
}
if (result)
{
//Debug.Log ("Pathfinding time: " + (sw.ElapsedMilliseconds) + " ms");
FpsCounter.pathfindingTime = sw.ElapsedMilliseconds;
}
return(pathResult);
}
/// <summary>
/// Should only be called from unity events such as Start or Update.
/// The requests are actually scheduled at the end of LateUpdate.
/// Requests are returned (once completed) before Update.
/// For example, a request is made in frame 0 in the Update thread. The path takes 1 frame to calculate.
/// Then, in frame 2, before any calls to Update, the return method is called.
/// </summary>
/// <param name="request"></param>
public void Enqueue(PathfindingRequest request)
{
if (request == null)
{
return;
}
if (!pending.Contains(request))
{
pending.Add(request);
}
else
{
Debug.LogWarning("That pathfinding request was already submitted.");
}
}
public void Update()
{
if (CurrentRequest != null)
{
return;
}
if (currentPath == null)
{
return;
}
movementTimer += Time.deltaTime;
PNode previousNode = currentPath[previousNodeIndex];
PNode nextNode = currentPath[previousNodeIndex + 1];
float dst = GetDistance(previousNode, nextNode);
float progress = Mathf.Clamp01((movementTimer * MovementSpeed) / dst);
if (progress == 1f)
{
// Update our position to the position we just reached.
this.X = nextNode.X;
this.Y = nextNode.Y;
if (previousNodeIndex == currentPath.Count - 2)
{
// Reached the end of the path. Request a new path...
// Setting current request also stops us from moving, see the first line of Update.
CurrentRequest = CreateNewRequest();
}
else
{
previousNodeIndex++;
movementTimer = 0f;
}
}
// Set position. Note that PNode can be used as a Vector. Useful and clean.
transform.position = Vector2.Lerp(previousNode, nextNode, progress);
transform.Translate(0f, 0f, -1f); // Move towards the camera.
}
protected override void Update(float processingTime)
{
if (subsystemQuery.Keys.Count <= 0)
{
//Run queued paths
if (pathRequests.Count == 0)
{
return;
}
//Get the request
PathfindingRequest request = pathRequests[0];
pathRequests.RemoveAt(0);
//Run the path
CalculatePath(request);
return;
}
//Get the query
string queryName = subsystemQuery.Keys.ElementAt(0);
object queryData = subsystemQuery[queryName];
//Execute queries
switch (queryName)
{
case "SetWorldConditions":
SetWorldConditions();
break;
case "DebugPath":
RequestDebugPath();
break;
default:
Log.PrintError($"Unrecognised query [{queryName}] in networkGenerator, deleting");
break;
}
subsystemQuery.Remove(queryName);
}
/// <summary>
/// Adds the initial nodes to check and updates them to what we need.
/// </summary>
/// <param name="nodeChecklist"></param>
private void SetupInitialConditions(ref List <Node> nodeChecklist, PathfindingRequest request)
{
Node firstNode = worldNodes[request.start_x, request.start_y];
//Setup the first node
firstNode.SetCosts(0, QDistanceCalculation(request.start_x, request.start_y, request.end_x, request.end_y));
firstNode.originNode = null;
firstNode.pathId = pathId;
firstNode.isChecked = true;
List <Node> firstNodes = GetAdjacentNodes(firstNode);
foreach (Node node in firstNodes)
{
//The node is being worked on this path
node.pathId = pathId;
//We connect to the main node simply
node.originNode = firstNode;
//Calculate the costs.
node.SetCosts(1, QDistanceCalculation(node.x, node.y, firstNode.x, firstNode.y));
//Add it
nodeChecklist.Add(node);
}
}
private void UponPathCompleted(PathfindingResult result, List <PNode> path)
{
// This means that the request has completed, so it is important to dispose of it now.
CurrentRequest.Dispose();
CurrentRequest = null;
// Check the result...
if (result != PathfindingResult.SUCCESSFUL)
{
// Debug.LogWarning("Pathfinding failed: " + result);
// Most likely is that it was impossible to find a route from the start to end.
// Request a new path then, hopefully this time it won't be a failure.
CurrentRequest = CreateNewRequest();
}
else
{
// Apply the path that was just calculated.
currentPath = path;
previousNodeIndex = 0;
movementTimer = 0f;
}
}
/// <summary>
/// Calculates the face path.
/// </summary>
/// <param name="request">The request to calculate.</param>
/// <param name="surface">The surface to be used for calculations.</param>
/// <returns>The face path in the form of a list.</returns>
public static List <IImmutableFace> FindFacePath(PathfindingRequest request, NavigationSurface surface)
{
int agentAreaMask = request.areaMask;
Face start;
if (request.startFace != null)
{
start = (Face)request.startFace;
}
else
{
start = surface.FindFirstFaceUnderPosition(request.startPosition, agentAreaMask);
}
Face end = surface.FindFirstFaceUnderPosition(request.endPosition, agentAreaMask);
if (start == null || end == null)
{
//Debug.Log((start == null) + " " + (end == null) + " null null FaceAStar");
return(null);
}
else if (start == end)
{
//Debug.Log("start == end FaceAStar");
return(new List <IImmutableFace>()
{
start
});
}
else
{
//Debug.Log("process FaceAStar");
var process = new FaceAStar(start, end, agentAreaMask);
process.FindFacePath();
return(process.result);
}
}
public List<PathNode> RequestPath(PathfindingRequest req, ref bool result)
{
List<PathNode> pathResult = null;
//float time1 = 0;
//float time2 = 0;
//DateTime dt1 = DateTime.Now;
//DateTime dt2 = DateTime.Now;
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
if (FindRequest(req) != null && FpsCounter.pathfindingsCounter <= maxPathFindingPerSecond)
{
FpsCounter.pathfindingsCounter++;
requests.Remove(req);
result = true;
sw.Start();
pathResult = GetPath(req.Start, req.End);
sw.Stop();
}
else if (requests.Count == 0 && FpsCounter.pathfindingsCounter <= maxPathFindingPerSecond)
{
FpsCounter.pathfindingsCounter++;
result = true;
sw.Start();
pathResult = GetPath(req.Start, req.End);
sw.Stop();
}
else
{
result = false;
requests.Add(req);
pathResult = null;
}
if (result)
{
//Debug.Log ("Pathfinding time: " + (sw.ElapsedMilliseconds) + " ms");
FpsCounter.pathfindingTime = sw.ElapsedMilliseconds;
}
return pathResult;
}
/*public static int Closest(List<PathNode> inNodes, Vector3 toPoint)
{
int closestIndex = 0;
float minDist = float.MaxValue;
for(int i = 0; i < inNodes.Count; i++)
{
if(AStarHelper.Invalid(inNodes[i]))
continue;
float thisDist = Vector3.Distance(toPoint, inNodes[i].Position);
if(thisDist > minDist)
continue;
minDist = thisDist;
closestIndex = i;
}
if (minDist != float.MaxValue)
return closestIndex;
else
return -1;
}*/
/*public static List<PathNode> GetSources()
{
return sources;
}*/
PathfindingRequest FindRequest(PathfindingRequest req)
{
foreach (PathfindingRequest pr in requests)
if (req.Id == pr.Id)
return req;
return null;
}
public PathfindingRequest(PathfindingRequest _ref)
{
this.start = _ref.start;
this.end = _ref.end;
this.id = _ref.id;
}
/// <summary>
/// Calculates the path.
/// We can do it all in 1 go, this system is on a seperate thread so it doesn't matter too much if we take a while.
/// This is probably a pretty ineffecient implementation but its on a seperate thread, gets the job done and isn't too critical for the project.
/// </summary>
/// <param name="request"></param>
public bool CalculatePath(PathfindingRequest request)
{
pathId++;
//The list of nodes that can be searched
//Everything in this list should have updated costs values before going in
//since we don't reset and recaclulate every time so the old path values will
//be carried over otherwise.
List <Node> searchNodes = new List <Node>();
//Cache the end node because its quicker than checking list every time
Node targetNode = worldNodes[request.end_x, request.end_y];
//Calculate initial nodes (The ones adjacent)
SetupInitialConditions(ref searchNodes, request);
//Cool lets just keep searching until we run out of things to search
//If we have to search more than X nodes, just assume its impossible.
int sanity = 20000;
Stopwatch timer = new Stopwatch();
timer.Start();
while (sanity > 0 && searchNodes.Count > 0)
{
sanity--;
//Might be a way with cachine to make this faster, this is quite slow :(
//We could use a sorted list, binary search and insertion in order?#
//Would be tons faster
//(We do all this mini optimisations then have this as super slow!)
int lowestIndex = 0;
float lowestCost = searchNodes[lowestIndex].GetTotalCost();
for (int i = 1; i < searchNodes.Count; i++)
{
float testCost = searchNodes[i].GetTotalCost();
if (testCost < lowestCost)
{
lowestCost = testCost;
lowestIndex = i;
}
}
//We know the node to search now
Node testingNode = searchNodes[lowestIndex];
//Console.WriteLine($"Checking: {testingNode.x}, {testingNode.y} with cost: {testingNode.GetTotalCost()}");
//Have we reached the end?
if (testingNode == targetNode)
{
List <Node> quickestPath = testingNode.GetRecursiveNodes();
List <Turf> pathTurfs = new List <Turf>();
Log.PrintDebug($"Found path length: {quickestPath.Count}");
#if !UNITY_INCLUDE_TESTS
foreach (Node node in quickestPath)
{
Turf locatedBase = LevelGenerator.current.turfs[node.x, node.y];
pathTurfs.Add(locatedBase);
}
#endif
Path finalPath = new Path();
finalPath.calculatedRoute = pathTurfs;
request.CompletePath(finalPath);
timer.Stop();
Log.PrintDebug($"Found path in {timer.ElapsedMilliseconds}ms");
return(true);
}
//Add surrounding nodes
AddSurroundingNodes(ref searchNodes, testingNode, request);
//Block it from being re-added
testingNode.isChecked = true;
//Thats all the checking we needed to do
searchNodes.Remove(testingNode);
}
//No path found
request.FailPath();
Log.PrintDebug("No path was found");
Log.PrintDebug($"Failed to find path in {timer.ElapsedMilliseconds}ms");
return(false);
}
public PathfindingRequest(PathfindingRequest _ref)
{
this.start = _ref.start;
this.end = _ref.end;
this.id = _ref.id;
}
