515 lines
13 KiB
C++
515 lines
13 KiB
C++
#include <memory>
|
|
#include <stdio.h>
|
|
#include <vector>
|
|
#include "../../common/Log.h"
|
|
#include "pathfinder_nav_mesh.h"
|
|
#include <DetourCommon.h>
|
|
#include <DetourNavMeshQuery.h>
|
|
#include "../zoneserver.h"
|
|
#include "region_map.h"
|
|
#include "../client.h"
|
|
|
|
struct PathfinderNavmesh::Implementation
|
|
{
|
|
dtNavMesh *nav_mesh;
|
|
dtNavMeshQuery *query;
|
|
};
|
|
|
|
PathfinderNavmesh::PathfinderNavmesh(const std::string &path)
|
|
{
|
|
m_impl.reset(new Implementation());
|
|
m_impl->nav_mesh = nullptr;
|
|
m_impl->query = nullptr;
|
|
Load(path);
|
|
}
|
|
|
|
PathfinderNavmesh::~PathfinderNavmesh()
|
|
{
|
|
Clear();
|
|
}
|
|
|
|
IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, int flags)
|
|
{
|
|
partial = false;
|
|
|
|
if (!m_impl->nav_mesh) {
|
|
return IPath();
|
|
}
|
|
|
|
if (!m_impl->query) {
|
|
m_impl->query = dtAllocNavMeshQuery();
|
|
}
|
|
|
|
m_impl->query->init(m_impl->nav_mesh, 4092 /*RuleI(Pathing, MaxNavmeshNodes)*/);
|
|
glm::vec3 current_location(start.x, start.z, start.y);
|
|
glm::vec3 dest_location(end.x, end.z, end.y);
|
|
|
|
dtQueryFilter filter;
|
|
filter.setIncludeFlags(flags);
|
|
filter.setAreaCost(0, 1.0f); //Normal
|
|
filter.setAreaCost(1, 3.0f); //Water
|
|
filter.setAreaCost(2, 5.0f); //Lava
|
|
filter.setAreaCost(4, 1.0f); //PvP
|
|
filter.setAreaCost(5, 2.0f); //Slime
|
|
filter.setAreaCost(6, 2.0f); //Ice
|
|
filter.setAreaCost(7, 4.0f); //V Water (Frigid Water)
|
|
filter.setAreaCost(8, 1.0f); //General Area
|
|
filter.setAreaCost(9, 0.1f); //Portal
|
|
filter.setAreaCost(10, 0.1f); //Prefer
|
|
|
|
dtPolyRef start_ref;
|
|
dtPolyRef end_ref;
|
|
glm::vec3 ext(5.0f, 100.0f, 5.0f);
|
|
|
|
m_impl->query->findNearestPoly(¤t_location[0], &ext[0], &filter, &start_ref, 0);
|
|
m_impl->query->findNearestPoly(&dest_location[0], &ext[0], &filter, &end_ref, 0);
|
|
|
|
if (!start_ref || !end_ref) {
|
|
return IPath();
|
|
}
|
|
|
|
int npoly = 0;
|
|
dtPolyRef path[1024] = { 0 };
|
|
auto status = m_impl->query->findPath(start_ref, end_ref, ¤t_location[0], &dest_location[0], &filter, path, &npoly, 1024);
|
|
|
|
if (npoly) {
|
|
glm::vec3 epos = dest_location;
|
|
if (path[npoly - 1] != end_ref) {
|
|
m_impl->query->closestPointOnPoly(path[npoly - 1], &dest_location[0], &epos[0], 0);
|
|
partial = true;
|
|
|
|
auto dist = DistanceSquared(epos, current_location);
|
|
if (dist < 10000.0f) {
|
|
stuck = true;
|
|
}
|
|
}
|
|
|
|
float straight_path[2048 * 3];
|
|
unsigned char straight_path_flags[2048];
|
|
|
|
int n_straight_polys;
|
|
dtPolyRef straight_path_polys[2048];
|
|
|
|
status = m_impl->query->findStraightPath(¤t_location[0], &epos[0], path, npoly,
|
|
straight_path, straight_path_flags,
|
|
straight_path_polys, &n_straight_polys, 2048, DT_STRAIGHTPATH_AREA_CROSSINGS);
|
|
|
|
if (dtStatusFailed(status)) {
|
|
return IPath();
|
|
}
|
|
|
|
if (n_straight_polys) {
|
|
IPath Route;
|
|
for (int i = 0; i < n_straight_polys; ++i)
|
|
{
|
|
glm::vec3 node;
|
|
node.x = straight_path[i * 3];
|
|
node.z = straight_path[i * 3 + 1];
|
|
node.y = straight_path[i * 3 + 2];
|
|
|
|
Route.push_back(node);
|
|
|
|
unsigned short flag = 0;
|
|
if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
|
|
if (flag & 512) {
|
|
Route.push_back(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
return Route;
|
|
}
|
|
}
|
|
|
|
IPath Route;
|
|
Route.push_back(end);
|
|
return Route;
|
|
}
|
|
|
|
IPathfinder::IPath PathfinderNavmesh::FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions &opts)
|
|
{
|
|
partial = false;
|
|
|
|
if (!m_impl->nav_mesh) {
|
|
return IPath();
|
|
}
|
|
|
|
if (!m_impl->query) {
|
|
m_impl->query = dtAllocNavMeshQuery();
|
|
}
|
|
|
|
m_impl->query->init(m_impl->nav_mesh, 4092 /*RuleI(Pathing, MaxNavmeshNodes)*/);
|
|
glm::vec3 current_location(start.x, start.z, start.y);
|
|
glm::vec3 dest_location(end.x, end.z, end.y);
|
|
|
|
dtQueryFilter filter;
|
|
filter.setIncludeFlags(opts.flags);
|
|
filter.setAreaCost(0, opts.flag_cost[0]); //Normal
|
|
filter.setAreaCost(1, opts.flag_cost[1]); //Water
|
|
filter.setAreaCost(2, opts.flag_cost[2]); //Lava
|
|
filter.setAreaCost(4, opts.flag_cost[3]); //PvP
|
|
filter.setAreaCost(5, opts.flag_cost[4]); //Slime
|
|
filter.setAreaCost(6, opts.flag_cost[5]); //Ice
|
|
filter.setAreaCost(7, opts.flag_cost[6]); //V Water (Frigid Water)
|
|
filter.setAreaCost(8, opts.flag_cost[7]); //General Area
|
|
filter.setAreaCost(9, opts.flag_cost[8]); //Portal
|
|
filter.setAreaCost(10, opts.flag_cost[9]); //Prefer
|
|
|
|
static const int max_polys = 256;
|
|
dtPolyRef start_ref;
|
|
dtPolyRef end_ref;
|
|
glm::vec3 ext(10.0f, 200.0f, 10.0f);
|
|
|
|
m_impl->query->findNearestPoly(¤t_location[0], &ext[0], &filter, &start_ref, 0);
|
|
m_impl->query->findNearestPoly(&dest_location[0], &ext[0], &filter, &end_ref, 0);
|
|
|
|
if (!start_ref || !end_ref) {
|
|
return IPath();
|
|
}
|
|
|
|
int npoly = 0;
|
|
dtPolyRef path[max_polys] = { 0 };
|
|
auto status = m_impl->query->findPath(start_ref, end_ref, ¤t_location[0], &dest_location[0], &filter, path, &npoly, max_polys);
|
|
|
|
if (npoly) {
|
|
glm::vec3 epos = dest_location;
|
|
if (path[npoly - 1] != end_ref) {
|
|
m_impl->query->closestPointOnPoly(path[npoly - 1], &dest_location[0], &epos[0], 0);
|
|
partial = true;
|
|
|
|
auto dist = DistanceSquared(epos, current_location);
|
|
if (dist < 10000.0f) {
|
|
stuck = true;
|
|
}
|
|
}
|
|
|
|
int n_straight_polys;
|
|
glm::vec3 straight_path[max_polys];
|
|
unsigned char straight_path_flags[max_polys];
|
|
dtPolyRef straight_path_polys[max_polys];
|
|
|
|
auto status = m_impl->query->findStraightPath(¤t_location[0], &epos[0], path, npoly,
|
|
(float*)&straight_path[0], straight_path_flags,
|
|
straight_path_polys, &n_straight_polys, 2048, DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS);
|
|
|
|
if (dtStatusFailed(status)) {
|
|
return IPath();
|
|
}
|
|
|
|
if (n_straight_polys) {
|
|
if (opts.smooth_path) {
|
|
IPath Route;
|
|
|
|
//Add the first point
|
|
{
|
|
auto &flag = straight_path_flags[0];
|
|
if (flag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
|
|
auto &p = straight_path[0];
|
|
|
|
Route.push_back(glm::vec3(p.x, p.z, p.y));
|
|
}
|
|
else {
|
|
auto &p = straight_path[0];
|
|
|
|
float h = 0.0f;
|
|
if (dtStatusSucceed(GetPolyHeightOnPath(path, npoly, p, &h))) {
|
|
p.y = h + opts.offset;
|
|
}
|
|
|
|
Route.push_back(glm::vec3(p.x, p.z, p.y));
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < n_straight_polys - 1; ++i)
|
|
{
|
|
auto &flag = straight_path_flags[i];
|
|
|
|
if (flag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
|
|
auto &poly = straight_path_polys[i];
|
|
|
|
auto &p2 = straight_path[i + 1];
|
|
glm::vec3 node(p2.x, p2.z, p2.y);
|
|
Route.push_back(node);
|
|
|
|
unsigned short pflag = 0;
|
|
if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &pflag))) {
|
|
if (pflag & 512) {
|
|
Route.push_back(true);
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
auto &p1 = straight_path[i];
|
|
auto &p2 = straight_path[i + 1];
|
|
auto dist = glm::distance(p1, p2);
|
|
auto dir = glm::normalize(p2 - p1);
|
|
float total = 0.0f;
|
|
glm::vec3 previous_pt = p1;
|
|
|
|
while (total < dist) {
|
|
glm::vec3 current_pt;
|
|
float dist_to_move = opts.step_size;
|
|
float ff = opts.step_size / 2.0f;
|
|
|
|
if (total + dist_to_move + ff >= dist) {
|
|
current_pt = p2;
|
|
total = dist;
|
|
}
|
|
else {
|
|
total += dist_to_move;
|
|
current_pt = p1 + dir * total;
|
|
}
|
|
|
|
float h = 0.0f;
|
|
if (dtStatusSucceed(GetPolyHeightOnPath(path, npoly, current_pt, &h))) {
|
|
current_pt.y = h + opts.offset;
|
|
}
|
|
|
|
Route.push_back(glm::vec3(current_pt.x, current_pt.z, current_pt.y));
|
|
previous_pt = current_pt;
|
|
}
|
|
}
|
|
}
|
|
|
|
return Route;
|
|
}
|
|
else {
|
|
IPath Route;
|
|
for (int i = 0; i < n_straight_polys; ++i)
|
|
{
|
|
auto ¤t = straight_path[i];
|
|
glm::vec3 node(current.x, current.z, current.y);
|
|
Route.push_back(node);
|
|
|
|
unsigned short flag = 0;
|
|
if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
|
|
if (flag & 512) {
|
|
Route.push_back(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
return Route;
|
|
}
|
|
}
|
|
}
|
|
|
|
return IPath();
|
|
}
|
|
|
|
glm::vec3 PathfinderNavmesh::GetRandomLocation(const glm::vec3 &start)
|
|
{
|
|
if (start.x == 0.0f && start.y == 0.0)
|
|
return glm::vec3(0.f);
|
|
|
|
if (!m_impl->nav_mesh) {
|
|
return glm::vec3(0.f);
|
|
}
|
|
|
|
if (!m_impl->query) {
|
|
m_impl->query = dtAllocNavMeshQuery();
|
|
m_impl->query->init(m_impl->nav_mesh, 4092 /*RuleI(Pathing, MaxNavmeshNodes)*/);
|
|
}
|
|
|
|
dtQueryFilter filter;
|
|
filter.setIncludeFlags(65535U ^ 2048);
|
|
filter.setAreaCost(0, 1.0f); //Normal
|
|
filter.setAreaCost(1, 3.0f); //Water
|
|
filter.setAreaCost(2, 5.0f); //Lava
|
|
filter.setAreaCost(4, 1.0f); //PvP
|
|
filter.setAreaCost(5, 2.0f); //Slime
|
|
filter.setAreaCost(6, 2.0f); //Ice
|
|
filter.setAreaCost(7, 4.0f); //V Water (Frigid Water)
|
|
filter.setAreaCost(8, 1.0f); //General Area
|
|
filter.setAreaCost(9, 0.1f); //Portal
|
|
filter.setAreaCost(10, 0.1f); //Prefer
|
|
|
|
dtPolyRef randomRef;
|
|
float point[3];
|
|
|
|
dtPolyRef start_ref;
|
|
glm::vec3 current_location(start.x, start.z, start.y);
|
|
glm::vec3 ext(5.0f, 100.0f, 5.0f);
|
|
|
|
m_impl->query->findNearestPoly(¤t_location[0], &ext[0], &filter, &start_ref, 0);
|
|
|
|
if (!start_ref)
|
|
{
|
|
return glm::vec3(0.f);
|
|
}
|
|
|
|
if (dtStatusSucceed(m_impl->query->findRandomPointAroundCircle(start_ref, ¤t_location[0], 100.f, &filter, []() { return MakeRandomFloat(0.0,1.0); /*(float)zone->random.Real(0.0, 1.0);*/ }, &randomRef, point)))
|
|
{
|
|
return glm::vec3(point[0], point[2], point[1]);
|
|
}
|
|
|
|
return glm::vec3(0.f);
|
|
}
|
|
|
|
void PathfinderNavmesh::Clear()
|
|
{
|
|
if (m_impl->nav_mesh) {
|
|
dtFreeNavMesh(m_impl->nav_mesh);
|
|
}
|
|
|
|
if (m_impl->query) {
|
|
dtFreeNavMeshQuery(m_impl->query);
|
|
}
|
|
}
|
|
|
|
void PathfinderNavmesh::Load(const std::string &path)
|
|
{
|
|
Clear();
|
|
|
|
FILE *f = fopen(path.c_str(), "rb");
|
|
if (f) {
|
|
NavMeshSetHeader header;
|
|
size_t readLen = fread(&header, sizeof(NavMeshSetHeader), 1, f);
|
|
if (readLen != 1)
|
|
{
|
|
fclose(f);
|
|
return;
|
|
}
|
|
if (header.magic != NAVMESHSET_MAGIC)
|
|
{
|
|
fclose(f);
|
|
return;
|
|
}
|
|
if (header.version != NAVMESHSET_VERSION)
|
|
{
|
|
fclose(f);
|
|
return;
|
|
}
|
|
|
|
dtNavMesh* mesh = dtAllocNavMesh();
|
|
if (!mesh)
|
|
{
|
|
fclose(f);
|
|
return;
|
|
}
|
|
dtStatus status = mesh->init(&header.params);
|
|
if (dtStatusFailed(status))
|
|
{
|
|
fclose(f);
|
|
return;
|
|
}
|
|
|
|
// Read tiles.
|
|
for (int i = 0; i < header.numTiles; ++i)
|
|
{
|
|
NavMeshTileHeader tileHeader;
|
|
readLen = fread(&tileHeader, sizeof(tileHeader), 1, f);
|
|
if (readLen != 1)
|
|
{
|
|
fclose(f);
|
|
return;
|
|
}
|
|
|
|
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, f);
|
|
if (readLen != 1)
|
|
{
|
|
dtFree(data);
|
|
fclose(f);
|
|
return;
|
|
}
|
|
|
|
mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
|
|
}
|
|
|
|
m_impl->nav_mesh = mesh;
|
|
|
|
LogWrite(MAP__INFO, 7, "Map", "Loaded Navmesh File [{%s}]", path.c_str());
|
|
}
|
|
}
|
|
|
|
void PathfinderNavmesh::ShowPath(Client * c, const glm::vec3 &start, const glm::vec3 &end)
|
|
{
|
|
/* auto &list = entity_list.GetNPCList();
|
|
|
|
for (auto &iter : list) {
|
|
auto npc = iter.second;
|
|
auto name = npc->GetName();
|
|
|
|
if (strstr(name, "PathNode") != nullptr) {
|
|
npc->Depop();
|
|
}
|
|
}
|
|
|
|
PathfinderOptions opts;
|
|
opts.smooth_path = true;
|
|
opts.step_size = RuleR(Pathing, NavmeshStepSize);
|
|
bool partial = false;
|
|
bool stuck = false;
|
|
auto path = FindPath(start, end, partial, stuck, opts);
|
|
|
|
for (auto &node : path) {
|
|
if (!node.teleport) {
|
|
NPC::SpawnNPC("PathNode 2253 1 0 1 2 1", glm::vec4(node.pos, 1.0));
|
|
}
|
|
}*/
|
|
}
|
|
|
|
dtStatus PathfinderNavmesh::GetPolyHeightNoConnections(dtPolyRef ref, const float *pos, float *height) const
|
|
{
|
|
auto *m_nav = m_impl->nav_mesh;
|
|
|
|
if (!m_nav) {
|
|
return DT_FAILURE;
|
|
}
|
|
|
|
const dtMeshTile* tile = 0;
|
|
const dtPoly* poly = 0;
|
|
if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) {
|
|
return DT_FAILURE | DT_INVALID_PARAM;
|
|
}
|
|
|
|
if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) {
|
|
const unsigned int ip = (unsigned int)(poly - tile->polys);
|
|
const dtPolyDetail* pd = &tile->detailMeshes[ip];
|
|
for (int j = 0; j < pd->triCount; ++j)
|
|
{
|
|
const unsigned char* t = &tile->detailTris[(pd->triBase + j) * 4];
|
|
const float* v[3];
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
if (t[k] < poly->vertCount)
|
|
v[k] = &tile->verts[poly->verts[t[k]] * 3];
|
|
else
|
|
v[k] = &tile->detailVerts[(pd->vertBase + (t[k] - poly->vertCount)) * 3];
|
|
}
|
|
float h;
|
|
if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
|
|
{
|
|
if (height)
|
|
*height = h;
|
|
return DT_SUCCESS;
|
|
}
|
|
}
|
|
}
|
|
|
|
return DT_FAILURE | DT_INVALID_PARAM;
|
|
}
|
|
|
|
dtStatus PathfinderNavmesh::GetPolyHeightOnPath(const dtPolyRef *path, const int path_len, const glm::vec3 &pos, float *h) const
|
|
{
|
|
if (!path || !path_len) {
|
|
return DT_FAILURE;
|
|
}
|
|
|
|
for (int i = 0; i < path_len; ++i) {
|
|
dtPolyRef ref = path[i];
|
|
|
|
if (dtStatusSucceed(GetPolyHeightNoConnections(ref, &pos[0], h))) {
|
|
return DT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
return DT_FAILURE;
|
|
}
|