Multi-goal shortest path

[erikwijmans]

Summary

Adds the ability to cache a the open/closed sets from a call to findPath. This is useful as you can then very quickly get to path to a new goal from the same starting location.

Changes to core search algorithm to support multiple starting locations. Multiple start searching has little extra code complexity and no extra asymptotic complexity. Multiple start also allows multiple goal as you can simple search from goals to start and then reverse the path.

Changes

## Caching support changes

Adds a dtQueryPathState struct which contains its own version of the openList and nodePool and the list of starting points used for the last search. findPath now takes an optional pointer to a dtQueryPathState. If given a valid pointer, it will use the openList and nodePool in the dtQueryPathState and will reuse any partial state if possible. Otherwise, it will use the openList and nodePool provided by dtNavMeshQuery and function as it does currently.

Adds dtAllocQueryPathState and dtFreeQueryPathState to support the allocation and deletion of the search states.

Multi-start support changes

Adds a new method to dtNavMeshQuery called findPathFromAny and re-implements findPath as a special case of findPathFromAny.

findPathFromAny takes the number of starting locations as its first argument, and then all arguments relating to the start locations (poly refs and (x, y, z) locations) are now lists. The returned path will start at the closet (minimal cost/geodesic distance) starting location to the goal.

Testing

These changes are backwards compatible with all existing tests and code.

To test multi start

query->findPathFromAny(numStartingLocations, startPolyRefs,
        endPolyRef, startPoses, endPos, ....);

P.S. Sorry for the large diffs, I left my whitespace trimmer on and didn't notice until I had made too many changes to turn it off :(
This is a much simpler PR than the number of changes would indicate.

[jakobbotsch]

Thanks for the contribution.

From a cursory glance, it seems like you are introducing allocations in the default case to some of the most commonly used methods in the library. This is not a change we can accept. The dtQueryPathState class and parameters your are introducing seem largely unnecessary anyway -- the preferred way (if we were to want these features, see below) would be to simply have the user create multiple dtNavMeshQuery instances.

However I am not quite sold on these features. First of, the caching: I don't think the A* heuristic used on the old nodes is going to be admissible when you try to search for a path to a new destination, so I am not convinced that you can reuse cached nodes and be guaranteed to get an optimal path. Additionally I don't see you handling the case where the user performs searches from different points within the same polygons (maybe this does not matter due to convexity? Not sure offhand). Also, you are not handling the fact that the user may update the navmesh between path queries, which will invalidate all node state.

In any case I'm not sure this is the best way to accomplish this. The most obvious usage of this kind of thing is to find paths to multiple destinations from a single start point. But for this purpose we already have Dijkstra searches (findPolyAround*) and a well-documented helper method (getPathFromDijkstraSearch) to obtain paths from such a search. If you have few destinations your solution may be faster, but I don't think it's worth the potential headaches with keeping the data consistent.

With regards to the multi-start queries: this is not a feature I have seen requested before. It seems rather strange to find a path from any start point rather than a path from all start points. Do you have a use-case in mind for this?

[erikwijmans]

Caching

1. Allocating memory in the default call. Yep, that was a poor implementation decision on my part. I can fix that.

2. Can the old nodes be used for a search to a new destination? Yes, they can. After an A* search returns, there are two types of nodes, reachable nodes (in the closed set or in the open set), and unreachable nodes. Unreachable nodes aren't of concern, so I will address the reachable nodes. For nodes that are closed, those certainly can be reused as the path from start to any of them is already guaranteed to be minimal. For nodes in the open set, those nodes can be re-used, but the heuristic must be recalculated and the nodes must be resorted based on their new total cost, which is being done.

3. Same polygon but a different point. This is something I hadn't considered and thank you for pointing it out! My current thought on that is that I can come up with contrived examples where the returned path would be incorrect. I am not sure if those cases are possible in practice, but probably the best thing to do is change the interface for a search with caching to only take polygon references (much like the Dijkstra interface), or think about how to fix this.

However, the bigger question is "Does recast need/want A* caching?". The use case for A* caching instead of Dijkstra is fairly slim, so the answer is probably not. I can drop that addition from this PR.

Multi-start

Multi-start queries. Multi-start queries have a very important use case for implementing the much more important/desirable multi-goal query. If the navigation graph is made of solely undirected edges (which it commonly is), then a shortest path is valid/minimal in both directions (start to end or end to start). This allows you to perform a multi-goal query by searching from goals to start and then reversing the resulting path. This is what I am currently using the multi-start query to perform. Based on Issue #106, it does seem that others are interested in multi-goal shortest paths.

For my use cases, multi-start is essential (due to multi-goal). I can expand this PR to include multi-start like functionality for Dijkstra. Does cutting caching and expanding multi-start seem reasonable for recast's wants/needs?

[jakobbotsch]

For nodes that are closed, those certainly can be reused as the path from start to any of them is already guaranteed to be minimal.

These nodes are minimal with respect to the cost to get to them plus the distance to the previous target.
It doesn't seem immediately obvious that this implies that they are also minimal for the new target, though I think you might be right.

If the navigation graph is made of solely undirected edges (which it commonly is), then a shortest path is valid/minimal in both directions (start to end or end to start). This allows you to perform a multi-goal query by searching from goals to start and then reversing the resulting path. This is what I am currently using the multi-start query to perform.

That makes sense. I suppose this is faster than simply using Dijkstra for your use-case?
The only thing that worries me here is that, as you say, this is only valid for undirected nav meshes. The thread you link to implements "correct" multi-goal search, that properly supports unidirectional jump-links. I would typically advice to use Dijkstra search in these cases. As you can tell, I am not a big fan of adding another less-general way of accomplishing this, but I am easily convinced if this is worth it in your case. It is a quite elegant addition to the search. 😄

Does cutting caching and expanding multi-start seem reasonable for recast's wants/needs?

I think that would be a good way to continue. I would still like some way to clarify that this is a search from any of the starting points -- perhaps by naming it something like findPathFromAny?

[erikwijmans]

I did some testing for my use case and it seems adding caching was premature optimization :)

I severally underestimated just how fast A* is on the navmesh is compared to the setting I used previously (A* on a fine occupancy grid) for a very similar use case. Caching reduces runtime by about 5% in my tests and that certainly isn't worth the extra code complexity.

I have changed to PR to just be an addition of multi-start to findPath and have renamed the method to findPathFromAny.

[jakobbotsch]

What about using Dijkstra vs findPathFromAny and reversing the paths? What differences do you see there?

[jakobbotsch]

Also, I would prefer if you could submit the whitespace changes (which I don't mind) as a separate PR, or submit this PR without them.

[erikwijmans]

If you are only interested in the path from the closest start (maybe the method should be findPathFromClosest?), then it’s a faster (it’s still A*, so the normal benefits still apply) and, IMO, a much more elegant than looping over all starts and taking the shortest solution.

[jakobbotsch]

I understand that, but it is not a use case I have seen requested before, and it sounds like you are using it to find paths to one of X goals by flipping the path found. I'm reluctant to add it if this is the only use since it does not work in general for this purpose (only with undirected graphs as you mention). In that case it would be better to implement a reverse A* that uses your same trick. On the other hand this will not be too easy due to the way unidirectional links are stored. That is something we should consider when doing a new major version.

So essentially I just want to think a little about this before introducing new APIs that will need to be supported. Do you have some concrete use-cases for "some-source-single-destination" path finding?

[memononen]

@jakobbotsch some old & dusty memories on the topic (in Fast Show drunk guy voice) :)

Multi-target pathfinding is sometimes used to pick closest goal. I.e. goto nearest health pack or goto cover or goto exit (in crowd sim), or to plan longer term goals (i.e. which base to attack).

Those above decisions are generally not just distance based, but the evaluation has other attributes too (i.e. distance to enemy, etc). Or in the case of covers and pickups it is often faster to first create a "mask" of locations which the agent can reach, and the look up if there are pickups or covers at those locations (i.e. bloom filter + poly ID hashmap -> cover). Also, it has more predictable performance, since you wont try to path find path to that health pack which is 5m away, across a river that cannot be crossed.

For longer term tactics, i've found that is usually cheaper to do time sliced individual path queries because that uses less memory. This also allows you to early out based on decision cost or time budget.

So, my observation has been that the multi target path query is something that often comes to mind first when solving multi-target problems, but in the end it ends up too costly or inflexible solution.

[jakobbotsch]

@memononen I definitely agree that finding paths from one source to the closest goal can have many use cases, but this PR is adding the ability to find paths from the closest source to a single goal. I am having trouble thinking of use cases for this scenario. Maybe you have some thoughts on that?

With re. to finding reachable polygons I have considered adding a data structure to do this, similar to #352 except supporting unidirectional offmesh connections. It can answer queries such as "is there are path from A to B?" efficiently. The main problem is that it is not dynamic and has to be recomputed if the nav mesh changes. But I have personally used such a data structure heavily in my projects indeed to avoid the unpredictable performance resulting from finding paths to unreachable locations.

[memononen]

@jakobbotsch I think a common way to implement multi-target is to seed multiple start locations for A* and reverse path. My understanding is that that is what this PR does. My point was that this type of search often feels like the right solution, but as more (game)logic gets added to the "find path to nearest goal", that particular solution never survives.

The "mask" in referred earlier meant array of polygons returned by findPolysAround() or similar constrainted dijkstra.

Something akin to #352 is super useful too, i agree. I once prototype hierarchical pathfinding which simplified each tile into a graph and then use the high level graph to find path or check accessibility. I think there was something related to query filter which I was not quite able to figure out.

[jakobbotsch]

@jakobbotsch I think a common way to implement multi-target is to seed multiple start locations for A* and reverse path. My understanding is that that is what this PR does. My point was that this type of search often feels like the right solution, but as more (game)logic gets added to the "find path to nearest goal", that particular solution never survives.

That makes sense. In my own applications I have often used path distance as a factor in which goal is chosen, but not at all as the only factor. For those situations I would just use a Dijkstra search.

That said, I am not against adding "multi-goal" functionality, even if you typically have more factors to consider than just path distance. But the "seed multiple start locations for A* and reverse path" is obviously incorrect for unidirectional off-mesh connections. And it is not too hard to do better, simply by writing an A* that considers edges reversed. So if this is the only use of multi-start, then I am reluctant to take the change.

Something akin to #352 is super useful too, i agree. I once prototype hierarchical pathfinding which simplified each tile into a graph and then use the high level graph to find path or check accessibility. I think there was something related to query filter which I was not quite able to figure out.

Oh right, I forgot about that. That is another problem with such a data structure -- you need one for essentially every combination of flags that you use. In my applications I did not have so many flags that it mattered, but it is maybe too many limitations to add it to the library. I have read some articles about dynamic connectivity data structures based on Euler tour trees which could be fun to try to implement.

[erikwijmans]

So if this is the only use of multi-start, then I am reluctant to take the change.

The only real use case I know of multi-start is to implement multi-goal on a graph with undirected edges.

it is not too hard to do better, simply by writing an A* that considers edges reversed.

I looked into this a little bit. It seems the way the edges are stored makes this challenging and I don't see a clean solution (if you have thought about this at all and can give me a pointer, I am happy to try this).

[jakobbotsch]

It seems the way the edges are stored makes this challenging and I don't see a clean solution (if you have thought about this at all and can give me a pointer, I am happy to try this).

I would probably try to do it in two steps:

1. Implement a findReversePath (or better named if you have any ideas) which starts at multiple goals and searches towards a beginning, ignoring any offmesh connection that are not bidirectional. This at least means the paths are always traversable, even if they are not always optimal. It should not be hard to skip those off-mesh connections (query dtPoly::getType for DT_POLYTYPE_OFFMESH_CONNECTION, obtain the off-mesh connection info with getOffMeshConnectionByRef, and check dtOffMeshConnection::con. You can inline getOffMeshConnectionByRef to make this check very cheap).
2. Update dtCreateNavMeshData to also include off-mesh connections that end in the tile. Update dtNavMesh::addTile to properly support this situation (it should simplify off-mesh connection a lot). Update path finds to take into account that links can now be "reverse-directed".

If you wish I am also ok with just a PR for step 1 and I can look at step 2 when I have time (though I have some backlog at the moment). We should also measure along the way, but off-mesh connections are typically rare so I don't think it will be too bad. As a bonus step 2 will allow us to support offmesh connections to tiles that are not direct neighbors (currently off-mesh connections are only supported to direct neighbors). Actually @memononen I am curious if this was a deliberate decision -- to only include off-mesh connections in the start tile. I realize it saves some space, but the hassle/limitations with connecting them seem significant, and compared to the rest of the data I would guess the space usage is negligible.

Of course this last step may require us to bump the version as well -- so it should probably go in the next major version.

Overall this seems like significant complexity, but at least step 2 would be nice to have IMO. It is something I have personally run into that should not be too hard to fix (although there are other ways to fix it). Thoughts @memononen?

[memononen]

@jakobbotsch There are two reasons for the off-mesh connection limitation, both of which can be improved. Firstly, memory management. I wanted to avoid allocations at runtime, so currently the link pool is allocated along with the data. I think this was bad call. Secondly, wanted to simplify the link removal code. You need to remove the links from source and destination tile, so just checking the neighbours remove the bookkeeping.

At Unity we modified Detour and removed the links from the data, which allowed the tile data to be completely read-only. This meant that there was one shared link pool and we also had all the off-mesh connections dynamically allocated. Having that one link pool was key to make it work. IIRC offmesh connections had separate book keeping so you could look up which connections fell into which tile, so links could be added removed as tiles changed.

It was extensive rewrite, but I think it was well worth it. It meant more allocations and in some cases more fragmentation, but that happened at predictable times: when adding and removing tiles or off-mesh connections.

[erikwijmans]

Took a stab at 1) One thing that I thought of while reimplementing to deal with whitespace issues is that I hadn't accounted for the case of many positions in the same polygon. This is now accounted for. I also added an optional argument to multi-goal/multi-start that tells you which of the goal/start polygons was found as if there are many positions in the same polygon, there is no way for the caller to know which was found.

[erikwijmans]

I was sleuthing around the code attempting to figure out a way to do 2) without requiring a large rewrite or version bump and I think I found one (although I don't particularly love it):

When building the navmesh, enough links are allocated for every off-mesh connection to be bidirectional (DetourNavMeshBuilder.cpp:385). Since links are built at load-time instead of being saved to disk, we can modify dtNavMesh::connectExtOffMeshLinks to create a dummy link represent a backwards traversal.

This would probably require modifying dtLink to include a flag to indicated it's a backwards traversal link and, without default initialization, wouldn't be backwards compatible with existing user code that creates dtLink's (although comments imply this is rare). I haven't found an already existing field in dtLink that could safely also store this information.

This is also a reasonably dangerous change as the backwards traversal links will look identical to normal links unless the flag is checked, so I imagine there is a fair amount of existing code that will need to be updated to check this flag and to not traverse backwards links.

Thoughts?

[jakobbotsch]

The number of states is severely limited (it is a 2 bit value). This will not work unfortunately.

[erikwijmans]

I didn't notice that, good catch.

Currently, 24 bits are allocated for the pidx, 2 for state, and 3 for flags. So there are 3 free bits, which would give 32 states and support for 32 goals.

With some additional logic, I could also use the state to just delineate different starts within a polygon instead of putting each start at a unique state. We could then decide how many of the free bits to allocated to state (if any) based on how many starts per polygon makes sense to support.

[jakobbotsch]

I don't think it should be necessary to insert all n nodes into the hash table. It should be ok to just insert the first one and only allocate the rest without inserting them in the hash table. I.e.

1. Add a public dtNodePool::allocNode
2. Update dtNodePool::getNode to use this
3. Allocate nodes directly when inserting start points

Whether all of this complexity is worth it for this feature I am not sure.

With regards to getting the index, it would maybe be possible to use dtNode::pidx, to store the index by adding another flag (or similar), again with a complexity trade-off.

[erikwijmans]

Gave this a shot. I also removed the functionality the return which goal was found as there wasn't a clear simple/clean way to do it.

[jakobbotsch]

I will need to look a little closer at this before I feel comfortable merging it, but I think it looks good.
Apologies for the slowness.

[erikwijmans]

I’ve changed this block of code as there aren’t enough bits in the state feild. Did you mean to comment on the PR as a whole?

[jakobbotsch]

I mean the addition of the allocNode and the interactions with potentially not keeping nodes in the hash table.

[jakobbotsch]

When building the navmesh, enough links are allocated for every off-mesh connection to be bidirectional (DetourNavMeshBuilder.cpp:385). Since links are built at load-time instead of being saved to disk, we can modify dtNavMesh::connectExtOffMeshLinks to create a dummy link represent a backwards traversal.

Unfortunately the multiplication by 2 you are seeing in that code is because each off-mesh connection is a polygon that is bidirectionally linked to its starting poly (by baseOffMeshLinks). Either way it has not been a requirement that off-mesh connections that end in a tile need to be passed to dtCreateNavMeshData so it would be hard to have users update to this and ensure it to be the case (even with a version update). The global link pool suggested by @memononen is probably the best idea. @memononen did you keep the neighbor-only limitation in Unity or add bookkeeping for this? As you can tell I am also interested in getting rid of this :)

This is also a reasonably dangerous change as the backwards traversal links will look identical to normal links unless the flag is checked, so I imagine there is a fair amount of existing code that will need to be updated to check this flag and to not traverse backwards links.

You are right that storing it together with other links would be bad for existing code. It is probably ok to expand dtPoly. There are also some bits available in dtPoly::vertCount. It would also be possible to do something more clever by using the value of dtPoly::firstLink, but it is complicated by the fact that a poly could have no regular links but still have reverse links. But this may all be outdated if we end up doing the global link pool.

[erikwijmans]

Looks like the tests on travis failed for some reason related to setting up the environment, pushed a dummy change to re-run them.

[jakobbotsch]

Sorry, I have missed that you have made the changes (by the way, no need to force push -- I will just squash it before merging, and it makes it easier for me to review changes if you don't).

I have a few changes. Other than that I would like to see some numbers on short path finds (using dtNavMeshQuery::findPath), just to make sure everything works and we aren't regressing something unexpectedly.

[erikwijmans]

Timing procedure: On a reasonably small navmesh (the paths used for this test have between 2 and 15 polygons), sample 3000 pairs of points that have a path between them. Then for each pair of points, compute the shortest path between them 4000 times and time that entire loop (reduce the effects clock overhead). All timing done in nanoseconds with std::chrono::high_resolution_clock.

Current recast master:
Average time per call to findPath: 2584.74

On this PR:
Average time per call to findPath: 2576.08 nanoseconds
Average time per call to findBidirPathToAny: 2771.6 nanoseconds (This is still just find a path a single point, but now measuring overhead of the extra checks and reversal)

There does seem to be a measurable overhead in the extra checks and reversal when using findBidirPathToAny when findPath would suffice, but there doesn't appear to be a measurable change in performance between findPath on master and findPath on this PR.

[jakobbotsch]

I have finally had some time to play with this feature and there are a few things that makes it very confusing to use as a user. The biggest is because with partial paths, the return values do not really make any sense the way the API is described:

Here, the path returned starts somewhere between start and end and then goes to the end. This is really surprising behavior compared to findPath.

The second thing is because the API does not return which goal was found. It seems to me that it makes the API almost unusable for meshes with large polygons (where multiple goals may be on the same polygon). You can get a path, but it is then very inconvenient to actually continue using the path because it is hard for you to know which end point it is belonging to. I am curious how you are resolving this in your own projects?

[erikwijmans]

Yeah, partial paths are unfortunately not something that can be resolved easily.

In terms of not returning the goal found, we tend to have quite small polygons, and thus we rarely have more than one target location per polygon. If there is more than one goal per polygon, I just brute-force search for the closest one in the findStraightPath stage.

[erikwijmans]

RE determining which goal node was found: I haven't thought about this enough to be 100% certain that it works, but I think the goal node found will be the one that is closest by L2 distance to the location of path[pathCount - 2]. That is a fairly simple/cheap thing and could be easily added -- assuming there isn't something I am missing and it actually works.

[erikwijmans]

This wasn't as simple as I thought as you need get the value of crossSide to get the correct position for path[*pathCount - 2]. It does pass a test I wrote that uses a different method that I am sure works (I just increased the value of DT_NODE_STATE_BITS and put the index of each goal there) to generate ground truth. To generate the set of goals, I sampled a couple points, and then randomly jittered each of them a couple times to ensure there were multiple end points in the same polygon.

[jakobbotsch]

Thank you for the attempts, but this change turned out to be more complex than I would have anticipated. Unfortunately it still comes with some rather particular "quirks" from the user's perspective (partial paths, bidirectionality and the "goal" finding which I am also not too sure about). For those reasons I don't really think it is worth it to integrate this into the library in the current state. The crux of the matter being that Detour's data is simply not set up to handle this kind of query in a natural way so that it can come without surprises to users.
I realize you have spent a lot of time and I am sorry about that, but unfortunately stuff has just gathered up to a point where I don't think it is worth it anymore.

[erikwijmans]

Yeah, I agree that this has gotten a too convoluted and has too many caveats for there to be a nice user experience. I have been implementing this as a reversed as that provides an elegant algorithm, but has one caveat I knew about (requiring bidirectional edges) and one I didn't know about (partial paths not making any sense). There is also the complication of figuring out which goal was found. With the way recast is currently structured, none of these caveats and complications are easy to deal with.

I will take one more shot at this functionality however: Finding the closest goal from a list of goals can also be implemented with a forward search. This isn't as nice of an algorithm, and it has worse worst case asymptotic complexity (O(N * num_goals) as opposed to staying O(N)). However, it doesn't have the caveats and complications that a reverse search does. There are two caveats that I know of 1) if you have more than O(log(N)) goals, you'd be better of doing a full dijkstra expand, 2) partial paths will still be kinda weird as the goal the algorithm believes will be the closest can change as you increase the length of the search.

One note: The way I am constructing a multi-goal heuristic is by taking the min of all the individual heuristics. This will result in an admissible heuristic (if all the individual ones are admissible) and a consistent heuristic (once again, if all the individual ones are). So A* will work as expected.

If forward search seems like a more reasonable way of achieving this functionality, I can clean up the code a bit and add some comments.