Pathfinding using Detour
When building more complex simulations, we might bump into the need to add some pathfinding behaviour. For the purposes of this tutorial we will be looking at Recast/Detour, and how we can use this library in an Simulate simulation.
Recast is used to build a navigation mesh (nav mesh), while Detour is used to find paths in nav meshes. In this tutorial we will only be looking at Detour, and assume we already have a nav mesh.
Note: Even though our tutorial is mostly about Detour, most of the things described here can still be reused to integrate with other pathfinding libraries.
There are a few ways we can go about implementing pathfinding in an Simulate simulation.
One of the easiest ways in which we can do pathfinding is to store our nav mesh in the Global State. The way an entity can find a path from point A to point B, is by sending a message to the Global State asking it to compute the path. When the operation is done, the Global State will send back a message with the result.
This works really well in most cases, however your simulation performance can degrade if:
- The nav mesh is too big
- There are lots of entities that make queries
The next sections will look at how we can tackle these two problems if they do occur.
One way to deal with the problems outlined in the previous section is to partition your map into smaller nav meshes. This means that each cell (which models a region in your space) will have a nav mesh of the region it is simulating. Since entities and the map are local to a cell, there is no need send messages, and entities can directly perform pathfinding operations.
This also works really well, however there is one big problem with this approach: there is no way to do cross-cell pathfinding!
So what happens if we have a big nav mesh, and want to do cross-cell pathfinding? We can take a mixed approach, where we keep the local nav meshes, and we also create a less-detailed nav mesh which we can store in the Global State.
Let's look at a how this would roughly work: let's assume we want to find the path from point A to point B. If A and B are in the same cell, then we can easily find the path by using the local nav mesh. What happens if A and B are in different cells? The Global State nav mesh can store "links" between cells. If you want to go from A (which is in Cell1) to B (Cell3), the Global State can check whether there are any links between neighbouring cells, such that you can get from Cell1 to Cell3. Let's take a look at the following diagram:
The Global State stores a navmesh that, for example, can find a path from Cell1 to Cell3. The output of that operation would be
[L1, L2], because in order to get from Cell1 to Cell3, you must go through these two links. When an entity wants to move from A to B (which are in different regions), it does the following:- Sends a message to the Global State, asking for the links from A to B
- Since the first link is shared between the cell of A and the next cell, theentity performs a local pathfinding operation to get from A to L1
- Repeat previous step until you get to B, which in our case means:
- Find a path from L1 to L2 using the nav mesh of Cell2
- Find a path from L2 to B using the nav mesh of Cell3
In this section we will look at setting up some code to implement the Global State approach to path finding.
Let's define our global state:
// ./Simulation/global_state.hh
#include <aether/global_state.hh>
#include <aether/user_state.hh>
#include "navmesh.hh"
class demo_global_state : public aether::global_state_base<octree_traits> {
std::unique_ptr<nav_mesh> m_mesh;
// ...
public:
demo_global_state(): m_mesh(new bin_nav_mesh("/tmp/navmesh.bin") { }
void process_messages(reader_type &reader, writer_type &writer) override;
std::vector<aether::global_state_base<octree_traits>::subscriber_topic_type> get_topics() override;
};
and
navmesh.hh:// ./Simulation/navmesh.hh
#include "user_defs.hh"
#include <DetourNavMesh.h>
#include <DetourNavMeshQuery.h>
#include <DetourNavMeshBuilder.h>
class nav_mesh {
protected:
dtNavMesh *m_nav_mesh;
dtNavMeshQuery *m_nav_query = dtAllocNavMeshQuery();
public:
virtual std::vector<aether::vec2f> find_path(aether::vec2f start,
aether::vec2f end);
virtual ~nav_mesh() {
dtFreeNavMesh(m_nav_mesh);
dtFreeNavMeshQuery(m_nav_query);
}
};
dtNavMesh* load_all(const char *path);
class bin_nav_mesh: public nav_mesh {
public:
bin_nav_mesh(const std::string &path) {
m_nav_mesh = load_all(path.c_str());
if (!m_nav_mesh) {
AETHER_LOG(ERROR)("Failed to load: ", path);
exit(EXIT_FAILURE);
}
m_nav_query->init(m_nav_mesh, 2048);
}
};
The
nav_mesh struct is our simplified interface to Detour's API. It defines a find_path operation, which gives you back a std::vector of coordinates that an entity should follow in order to get from start to end. The bin_nav_mesh struct is a helper that we created to load a navigation mesh from disk. The implementation (navmesh.cc) can be found in the appendix.Please note that:
- We assume that the nav mesh is on disk, which means this won't work when running the simulation remotely. In case we want to run this remotely we would need to change the implementation to use Simulate's asset storage capabilities.
- For the purposes of this tutorial, we are loading the nav mesh from a binary file that is created by RecastDemo.
For the sake of completion, let's instantiate our global state in our main function:
// ./Simulation/main.cc
int main(int argc, char *argv[]) {
// ...
aether::arguments arguments;
argument_parse(argc, argv, &arguments);
auto static_args = arguments.to_octree_params<octree_traits>();
// ...
static_args.configure_global_state<demo_global_state>();
// ...
}
Now that our nav mesh is loaded, we can go ahead and define our pathfinding-related messages:
// ./Simulation/user_defs.hh
struct request_path_message {
utin64_t request_id;
aether::vec2f start;
aether::vec2f end;
};
struct path_message {
uint64_t request_id;
std::vector<aether::vec2f> path;
};
// We need these structs to be serialisable
AETHER_SERDE_DERIVE_TRIVIAL(request_path_message)
AETHER_SERDE_DERIVE_TRIVIAL(path_message)
using message_t = std::variant<request_path_message, path_message>;
For now we assume that a message can be either a
request_path_message or a path_message. In a real world example we might have multiple messages that we can send around, which means our message_t would be different.Next step is to handle these requests, and send back responses:
// ./Simulation/global_state.cc
void demo_global_state::process_messages(reader_type &reader, writer_type &writer) {
while(auto maybe_message = reader.get_next()) {
const auto &message = maybe_message.value();
message_t msg;
if (message.get_payload_serde(msg)) {
if (auto fp = std::get_if<request_path_message>(&msg)) {
// find the path
auto result = m_mesh->find_path(fp->start, fp->end);
// prepare the result
path_message msg = {fp->request_id, result};
message_t payload(msg);
writer.set_payload_serde(payload);
writer.set_destination(aether::message::all_workers {});
const auto ret = writer.send();
assert(ret == 0 && "Message sending failed");
}
} else {
AETHER_LOG(INFO)("Invalid message received.");
}
}
}
// ...
What our global state does, is that it reads incoming messages, and if it finds a
request_path_message, it performs the pathfinding operation (by calling find_path on our nav_mesh struct), followed by a broadcast of the result to all workers. We do a broadcast request, because we are not sure where the entity that performed the pathfinding operation is. Workers can discard the path_message message if they are not interested. More information on worker messaging can be found here.Note: In this tutorial we are doing a naive match of variants. However a more elegant solution would be to use the visitor pattern.
This covers most of the implementation regarding pathfinding using the Global State. The cell-local approach to pathfinding should be easier to implement, since we don't need to deal with sending messages to other workers. The good thing is that the map loading functionality outlined in this section can be reused for the cell-local, and the mixed approach!
Let's assume that we want to update our map, like block off a path. The way we can model is to either refetch the map from our storage (assuming that an external event updates the map), or we can send a message to the Global State. We will be looking at the second case.
// ./Simulation/navmesh.hh
class nav_mesh {
protected:
// ...
static constexpr int FLAG_BLOCKED = 0x10;
public:
// ...
virtual void block_path_around(const aether::vec2f &position);
};
The implementation of
block_path_around is going to be in the Appendix at the bottom of this document.We also need to define a new message for our block operation:
// ...
struct block_path_message {
aether::vec2f coord;
};
// ...
AETHER_SERDE_DERIVE_TRIVIAL(block_path_message)
// Notice how this grew!
using message_t = std::variant<
request_path_message,
path_message,
block_path_message
>;
And then we need to handle it in our global state implementation:
void demo_global_state::process_messages(reader_type &reader, writer_type &writer) {
while(auto maybe_message = reader.get_next()) {
const auto &message = maybe_message.value();
message_t msg;
if (message.get_payload_serde(msg)) {
if (auto fp = std::get_if<request_path_message>(&msg)) {
// ...
} else if (auto p = std::get_if<block_path_message>(&msg)) {
m_mesh->block_path_around(p->coord);
}
} else {
AETHER_LOG(INFO)("Invalid message received.");
}
}
}
In a nutshell, all operations that we want to define on our nav mesh, will probably have an associated request message, possibly a response message, and will involve writing a bit of code to handle the message from within our global state.
// ./Simulation/navmesh.cc
std::vector<aether::vec2f> nav_mesh::find_path(aether::vec2f start, aether::vec2f end) {
dtQueryFilter filter;
filter.setAreaCost(RC_WALKABLE_AREA, 1.0f);
float nearest1[3], nearest2[3];
dtPolyRef p1, p2;
float extents[3] = {1.0f, 0.0f, 1.0f};
// Find the nearest polygon to our starting position
float s[3] = {start[0], 0, start[1]};
dtStatus status = m_nav_query->findNearestPoly(s, extents, &filter, &p1, nearest1);
if (status != DT_SUCCESS || p1 == 0) {
// start position not found
return {};
}
// Find the nearest polygon to our ending position
float e[3] = {end[0], 0, end[1]};
status = m_nav_query->findNearestPoly(e, extents, &filter, &p2, nearest2);
if (status != DT_SUCCESS || p2 == 0) {
// end position not found
return {};
}
// We want to exclude polygons that are set to BLOCKED
filter.setExcludeFlags(FLAG_BLOCKED);
// Find the polygons we have to go through from start to end.
dtPolyRef hops[200] = {};
int hops_len = 0;
status = m_nav_query->findPath(p1, p2, s, e, &filter, hops, &hops_len, 200);
if (status != DT_SUCCESS) {
// no path... we also assume that if we didn't find a path that's less
// than 200, we error
return {};
}
// Compute a list of coordinates that follow the polygons we found
// previously
const size_t len = 2048;
std::vector<float> path(len * 3);
std::vector<unsigned char> flags(len);
std::vector<dtPolyRef> refs(len);
status = m_nav_query->findStraightPath(s, e, hops, hops_len,
path.data(), flags.data(),
refs.data(), &hops_len, len);
// Prepare the path that we return to entities
std::vector<aether::vec2f> final_path;
final_path.reserve(hops_len);
for (size_t i = 0; i < hops_len; ++i) {
aether::vec2f node = {path[i * 3], path[i * 3 + 2]};
final_path.push_back(node);
}
return final_path;
}
void nav_mesh::block_path_around(const aether::vec2f &position){
dtQueryFilter filter;
filter.setAreaCost(RC_WALKABLE_AREA, 1.0f);
const float s[3] = {position[0], 0, position[1]};
const float extents[3] = {1.5f, 0.1f, 1.5f};
std::vector<dtPolyRef> poly_refs(128);
int n_poly_found;
// Get all the polygons that `position` touches
dtNavMeshQuery poly_query;
poly_query.init(m_nav_mesh, 2048);
poly_query.queryPolygons(s, extents, &filter, poly_refs.data(), &n_poly_found, 128);
poly_refs.resize(n_poly_found);
// Set the polygons that we found to BLOCKED, so that when finding a path,
// these are not taken into account
for(const auto& poly_ref : poly_refs) {
m_nav_mesh->setPolyFlags(poly_ref, FLAG_BLOCKED);
}
}
/// TAKEN FROM RECAST-DEMO
struct NavMeshSetHeader {
int magic;
int version;
int numTiles;
dtNavMeshParams params;
};
/// TAKEN FROM RECAST-DEMO
struct NavMeshTileHeader {
dtTileRef tileRef;
int dataSize;
};
/// TAKEN FROM RECAST-DEMO
static const int NAVMESHSET_MAGIC = 'M'<<24 | 'S'<<16 | 'E'<<8 | 'T'; //'MSET';
static const int NAVMESHSET_VERSION = 1;
/// TAKEN FROM RECAST-DEMO
/// Loads a binary file, such as the one output by RecastDemo.
dtNavMesh* load_all(const char* path) {
FILE* fp = fopen(path, "rb");
if (!fp) {
return nullptr;
}
// Read header.
NavMeshSetHeader header;
size_t readLen = fread(&header, sizeof(NavMeshSetHeader), 1, fp);
if (readLen != 1) {
fclose(fp);
return nullptr;
}
if (header.magic != NAVMESHSET_MAGIC) {
fclose(fp);
return nullptr;
}
if (header.version != NAVMESHSET_VERSION) {
fclose(fp);
return nullptr;
}
dtNavMesh* mesh = dtAllocNavMesh();
if (!mesh) {
fclose(fp);
return nullptr;
}
dtStatus status = mesh->init(&header.params);
if (dtStatusFailed(status)) {
fclose(fp);
return nullptr;
}
// Read tiles.
for (int i = 0; i < header.numTiles; ++i) {
NavMeshTileHeader tileHeader;
readLen = fread(&tileHeader, sizeof(tileHeader), 1, fp);
if (readLen != 1) {
fclose(fp);
return nullptr;
}
if (!tileHeader.tileRef || !tileHeader.dataSize) {
break;
}
unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
if (!data) break;
memset(data, 0, tileHeader.dataSize);
readLen = fread(data, tileHeader.dataSize, 1, fp);
if (readLen != 1) {
dtFree(data);
fclose(fp);
return nullptr;
}
mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
}
fclose(fp);
return mesh;
}