- Home /
Multithreaded pathfinder how do i start the pathfinding properly?
Iam making a pathfinder and currently iam in the process of multithreading it. I just started learning multithreading a few days ago. I still have to do some more refactoring (like programming to interfaces) but i seem to have made quite some process.
What i have done until now is implemented A* pathfinding. I created multiple instances of each pathfinder for each thread ensuring that any data that would be shared is separated from each other (except the data that doesnt change). There are a few variables that are shared so i putted locks around them. This seems to be working fine but needs (alot) more testing obviously.
The way the pathfinders currently get their next job doesnt feel right to me. Currently iam using a while loop in each pathfinder instance to check if there is a new path to be calculated. This wastes alot of cpu power. I could use Thread.Sleep to reduce the cpu load but that still doesnt feel right to me.
What i really want is that the pathrequest manager can sent a event to the pathfinders and give them a new path. I want this event to be invoked on the worker thread obviously. This seems like a very simple thing to me but i cannot figure out how.
PathRequestManager:
using RhinoCore.PathfindEngine.Interfaces;
using RhinoCore.PathFindEngine;
using System;
using System.Collections.Generic;
using System.Threading;
using TileMap.Controllers;
using UnityEngine;
namespace RhinoCore.PathfindEngine
{
public class PathRequestManager : MonoBehaviour
{
public static PathRequestManager Instance;
private readonly Queue<PathRequest> _pathRequestQueue = new Queue<PathRequest>();
private readonly Queue<CompletedPath> _pathCompletedQueue = new Queue<CompletedPath>();
private GridDataProvider2D _gridDataProvider2D;
private CompletedPath _lastCompletedPath;
private List<IPathfinder> _pathfinders;
private List<Thread> _threads;
private const int ThreadCount = 4;
private void Awake()
{
Instance = this;
_gridDataProvider2D = GetComponent<TileMapController2D>();
_pathfinders = new List<IPathfinder>();
_threads = new List<Thread>();
for (int i = 0; i < ThreadCount; i++)
{
var pathfinder = new AStarPathfinder(_gridDataProvider2D);
_pathfinders.Add(pathfinder);
_threads.Add(new Thread(() => pathfinder.StartFindPath()));
_threads[i].Start();
}
}
private void Update()
{
lock (_pathCompletedQueue)
{
if (_pathCompletedQueue.Count > 0)
{
var completedPath = _pathCompletedQueue.Dequeue();
completedPath.Callback(completedPath.Path, completedPath.Succes);
}
}
TryProcessNext();
}
public static void RequestPath(Vector3 pathStart, Vector3 pathEnd, Action<Vector3[], bool> callback)
{
var pathRequest = new PathRequest(pathStart, pathEnd, callback);
lock (Instance._pathRequestQueue)
{
Instance._pathRequestQueue.Enqueue(pathRequest);
}
}
private void TryProcessNext()
{
lock (_pathRequestQueue)
{
if (_pathRequestQueue.Count > 0)
{
for (int i = 0; i < _pathfinders.Count; i++)
{
if (_pathfinders[i].IsBusy) continue;
var pathRequest = _pathRequestQueue.Dequeue();
_pathfinders[i].SetPath(pathRequest);
}
}
}
}
public void FinishedProcessingPath(CompletedPath completedPath)
{
lock (_pathCompletedQueue)
{
_pathCompletedQueue.Enqueue(completedPath);
}
_lastCompletedPath = completedPath;
}
private void OnDrawGizmos()
{
if (_lastCompletedPath != null)
{
Gizmos.color = Color.green;
foreach (var pathNode in _lastCompletedPath.Path)
{
Gizmos.DrawCube(pathNode, new Vector3(1, 1, 1));
}
}
}
private void OnDisable()
{
foreach (var pathfinder in _pathfinders)
{
pathfinder.Stop = true;
}
}
}
}
AStarPathfinder:
using RhinoCore.Collections;
using RhinoCore.PathfindEngine;
using RhinoCore.PathfindEngine.Interfaces;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;
namespace RhinoCore.PathFindEngine
{
public class AStarPathfinder : IPathfinder
{
private readonly AStarGrid _grid;
private PathRequest _pathRequest;
public bool IsBusy { get; private set; }
public bool Stop { get; set; }
public AStarPathfinder(GridDataProvider2D gridDataProvider2D)
{
_grid = new AStarGrid(new Grid(gridDataProvider2D));
}
public void SetPath(PathRequest pathRequest)
{
lock (this)
{
_pathRequest = pathRequest;
}
}
public void StartFindPath(object threadConText = null)
{
while (!Stop)
{
lock (this)
{
if (_pathRequest != null)
{
IsBusy = true;
var completedPath = FindPath(_pathRequest);
IsBusy = false;
PathRequestManager.Instance.FinishedProcessingPath(completedPath);
}
}
}
}
public CompletedPath FindPath(PathRequest pathRequest)
{
var sw = new Stopwatch();
sw.Start();
var pathSucces = false;
var startNode = _grid.NodeFromWorldPoint(pathRequest.PathStart);
var targetNode = _grid.NodeFromWorldPoint(pathRequest.PathEnd);
if (startNode.Walkable && targetNode.Walkable)
{
var openSet = new Heap<IAStarNode>(_grid.MaxSize);
var closedSet = new HashSet<IAStarNode>();
int itterations = 0;
int neighbourUpdates = 0;
openSet.Add(startNode);
while (openSet.Count > 0)
{
itterations++;
IAStarNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
UnityEngine.Debug.Log(string.Format("Path found in {0} ms. Itterations: {1} Neighbourupdates: {2}", sw.ElapsedMilliseconds, itterations, neighbourUpdates));
_pathRequest = null;
pathSucces = true;
break;
}
foreach (var neighbour in _grid.GetNeighbours(currentNode))
{
if (!neighbour.Walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour))
{
neighbour.GCost = newMovementCostToNeighbour;
neighbour.HCost = GetDistance(neighbour, targetNode);
neighbour.Parent = currentNode;
neighbourUpdates++;
if (!openSet.Contains(neighbour))
openSet.Add(neighbour);
}
}
}
}
var waypoints = new Vector3[0];
if (pathSucces)
{
waypoints = RetracePath(startNode, targetNode);
}
else
{
UnityEngine.Debug.Log("Did not find a path :(");
}
return new CompletedPath(waypoints, pathSucces, pathRequest.Callback);
}
private Vector3[] RetracePath(IAStarNode startNode, IAStarNode endNode)
{
List<IAStarNode> path = new List<IAStarNode>();
var currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.Parent;
}
var waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return waypoints;
}
private Vector3[] SimplifyPath(List<IAStarNode> path)
{
List<Vector3> waypoints = new List<Vector3>();
Vector2 directionOld = Vector2.zero;
waypoints.Add(path[0].WorldPosition + new Vector2(0.5f, 0.5f));
for (int i = 1; i < path.Count; i++)
{
Vector2 directionNew = new Vector2(path[i - 1].GridX - path[i].GridX, path[i - 1].GridY - path[i].GridY);
if (directionNew != directionOld)
{
waypoints.Add(path[i - 1].WorldPosition + new Vector2(0.5f, 0.5f));
}
directionOld = directionNew;
}
return waypoints.ToArray();
}
private static int GetDistance(IAStarNode nodeA, IAStarNode nodeB)
{
var dstX = Math.Abs(nodeA.GridX - nodeB.GridX);
var dstY = Math.Abs(nodeA.GridY - nodeB.GridY);
return dstY + dstX;
}
}
}
Answer by Bunny83 · Jun 27, 2016 at 10:09 PM
Well, a few things that aren't quite right here ^^. First of all in general it's not a good idea to lock on "this". That's because "this" is the actual reference to your actual "AStarPathfinder" instance. That means someone from the outside could also aquire a lock on that object and might cause a dead-lock. If you need a lock inside a class you should lock on a private owned object. If you don't have one at hand you can always create one:
private object lockHandle = new object();
Of course locking your "_pathRequestQueue" is fine since it's an internal object that only your class can access.
To block a thread until it's needed you would use a "wait handle". There are multiple classes which can be used. One is the ManualResetEvent.
private ManualResetEvent waitHandle = new ManualResetEvent(false);
// inside your thread function:
while (true)
{
waitHandle.WaitOne();
// Process your pathfinding here.
waitHandle.Reset();
IsBusy = false;
}
public bool StartFindPath(PathRequest pathRequest)
{
if (!IsBusy)
{
_pathRequest = pathRequest;
IsBusy = true;
waitHandle.Set();
return true;
}
return false;
}
This way your thread function will wait until the event is set. You don't need any lock here. The busy flag prevents altering of the _pathRequest field during pathfinding.
So you would start your thread function right at the beginning. The thread will be blocked until an actual pathfinding request has been started. Once the request is done the wait handle is reset and ready to get the next request.
ps. Another tip for using locks: you should never hold a lock for a longer period of time. The way you use the lock inside your StartFindPath method will lock the entire object for the whole pathfinding time.
locks should be used to syncronise data. You usually just copy data inside the lock into a private variable to ensure the thread has it's own copy of the data that noone can change during it's operation. You should release locks as fast as possible. If you don't you won't gain anything from threading as other threads would just wait for your thread to finish. Also holding locks for too long is one of the main causes for dead locks.
Iam now using the waithandler and changed those lock(this) to use a private instance ins$$anonymous$$d. This seems to be mostly working fine and cpu usage is much lower when idle.
But iam getting this error sometimes: InvalidOperationException: Operation is not valid due to the current state of the object System.Collections.Generic.Queue`1[RhinoCore.PathfindEngine.PathRequest].Peek () System.Collections.Generic.Queue`1[RhinoCore.PathfindEngine.PathRequest].Dequeue () RhinoCore.PathfindEngine.PathRequest$$anonymous$$anager.TryProcessNext () (at Assets/Scripts/RhinoCore/PathfindEngine/PathRequest$$anonymous$$anager.cs:69) RhinoCore.PathfindEngine.PathRequest$$anonymous$$anager.Update () (at Assets/Scripts/RhinoCore/PathfindEngine/PathRequest$$anonymous$$anager.cs:48)
Pretty sure this is a threading related error. Iam locking that object though so iam wondering whats going wrong here.
EDIT: Ofcourse is giving this error. Iam checking if there is something in the queue before looping through and potentionaly dequeuing more than 1 item...which will give a error if there was only 1 item in the queue to begin with
Your answer
Follow this Question
Related Questions
astar' AI falls down, doesn't follow the grid 2 Answers
How to make AIPath (A*) work on 2D game ? 1 Answer
A* Pathfinding with mecanim and rotation 1 Answer
Look rotation makes sudden changes 1 Answer