Multithreaded pathfinder how do i start the pathfinding properly?

Archived Forums Questions & Answers
1

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*.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*.IsBusy) continue;
var pathRequest = _pathRequestQueue.Dequeue();
_pathfinders.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(_grid.MaxSize);
var closedSet = new HashSet();
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 path = new List();
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 path)
{
List waypoints = new List();
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_.GridX, path[i - 1].GridY - path*.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;
}
}
}

1 Answer

1
2

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.