Queueing with Unity's NavMeshAgent

Question

I have created a strategy game where the units use Unity's NavAgent pathfinding and flocking algorithms. This works well. However, I need to add queueing so that units will queue sensibly. This works most of the time, but it isn't perfect and I want to know if there's a better way of doing it.

My first draft was to simply check which unit is closest to the destination, but this caused issues because a group might be navigating around an obstacle and the unit closest is at the back of the queue... so the whole group stops.

Second draft checked the distance to destination based upon the length of the path each unit had to take. This works far more often, but sometimes still has issues. Sometimes the same problem happens, but much less often. The one who is supposed to be moving stops, and so the entire group stops.

There's also a problem I've noticed, where the waiting units could sometimes benefit from rotating, even if they shouldn't be moving. Unfortunately they sometimes get stuck and will wait before they can move, to rotate. If they could rotate when forbidden from moving it'd help considerably too.

What I need to know...

1. How can I make Unity NavAgent's queue, so that they will wait for the other agents closest to the destination to move first?

2. How can I ensure that this does not stop agents from rotating while in a queue (rotating may move them into a better position in the queue).

Code:

Unit Movement Function

private void Movement()
{
    // Unit is moving.
    if (moveVectors.Count > 0)
    {
        navAgent.isStopped = false;

        // Check for whether to wait in queue.
        Vector3 nextVector = transform.position;
                nextVector += transform.forward * speed * Time.deltaTime;

        foreach (Unit unit in list)
        {
            if (unit != this &&
                unit.moveVectors.Count > 0 &&
                Vector3.Distance(unit.transform.position, nextVector) <= (unit.footprint + footprint) &&
                PathDistance(unit) < PathDistance(this))
            {
                navAgent.isStopped = true;
                return;
            }
        }
    }
}

There's a bit more to Movement, but it's not relevant so we'll ignore it for now.

PathDistance Function

private float PathDistance(Unit unit)
{
    Vector3 startPosition = unit.transform.position;
    Vector3 endPosition = unit.moveVector;
    Vector3 lastPosition = startPosition;
    Vector3[] path = unit.navAgent.path.corners;
    float distance = 0;

    for (int i = 0; i < path.Length; i++)
    {
        distance += Vector3.Distance(lastPosition, path[i]);
        lastPosition = path[i];
    }

    distance += Vector3.Distance(lastPosition, endPosition);
    return distance;
}

Is there a better way of achieving the result I want?

Answer 1

Nearest-first movement priority

Second draft checked the distance to destination based upon the length of the path each unit had to take. This works far more often, but sometimes still has issues.

You don't say which issues, but I think we can assumed "ending up blocked". This is important, because this approach you mention is the only sane way to begin tackling the problem. Blocking probably happens because the shortest path is taking into account other units that are moving. You have to get the shortest path as if the map were otherwise empty. You can then prioritise who gets to move first in order of decreasing shortest-path proximity, i.e. closest first.

Inspiration for this idea is road traffic. Especially at stop streets, people don't constantly stop/start to wait for one another. They know when they have right of way, and they go confidently, otherwise they know they'll hold up traffic in the long run. I think the same applies in your case. If you get the shortest path in an otherwise clear map, you should not have this problem as no-one is trying to avoid anyone else, they are just moving to the goal - but in this case, they must do so in order of nearest to furthest proximity to the goal. We do the same when traffic merges into a single lane - we tend to let the guys who are already in front, go first. This assures a smooth flow, no accidents.

EDIT: At worst, a hybrid approach will be required where, perhaps if a unit does get locked up by the usual method, you occasionally attempt a straight move, or even a random movement to an available neighbouring cell, in order to let things settle into a nice queue - a bit like shaking up a tin to compact it and let the small stones go to the bottom. Knowing whether an agent is "locked up" is a matter of identifying patterns in its most recent moves.

General dev tips

If you're able to make your code run in interactive mode, that is, rather than the simulation running on freely, instead you press e.g. spacebar, then another entity moves, and the simulation waits for another press to move the next entity, so you are manually stepping through what would ordinarily be each frame of processing. If you aren't already doing so, you can then see / think through exactly what is happening with the logic (even more so if you hook up to VS debugger, which will allow you to step through the logic one instruction at a time, although Unity native assembly code may be inaccessible).

You also want to ideally be able to see the map (possibly per entity) showing A* pathing values..