mirror of
https://github.com/EQEmu/Server.git
synced 2026-09-01 07:26:36 +00:00
Some changes to navmesh path finding, disabled waypoint nav interface for now, might keep it disabled (i don't think anyones using it anyway) added a rule that might need some fine tuning for navmesh pathfinding
This commit is contained in:
KimLS
+246
-14
@@ -1,9 +1,9 @@
|
||||
#include <memory>
|
||||
#include <stdio.h>
|
||||
#include <vector>
|
||||
#include <DetourNavMesh.h>
|
||||
#include <DetourNavMeshQuery.h>
|
||||
#include "pathfinder_nav_mesh.h"
|
||||
#include <DetourCommon.h>
|
||||
#include <DetourNavMeshQuery.h>
|
||||
|
||||
#include "zone.h"
|
||||
#include "water_map.h"
|
||||
@@ -131,6 +131,177 @@ IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const gl
|
||||
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, 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(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[max_polys] = { 0 };
|
||||
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)
|
||||
@@ -310,24 +481,85 @@ void PathfinderNavmesh::Load(const std::string &path)
|
||||
|
||||
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 = FindRoute(start, end, partial, stuck);
|
||||
std::vector<FindPerson_Point> points;
|
||||
auto path = FindPath(start, end, partial, stuck, opts);
|
||||
|
||||
if (!partial) {
|
||||
FindPerson_Point p;
|
||||
for (auto &node : path)
|
||||
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)
|
||||
{
|
||||
if (!node.teleport) {
|
||||
p.x = node.pos.x;
|
||||
p.y = node.pos.y;
|
||||
p.z = node.pos.z;
|
||||
|
||||
points.push_back(p);
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
c->SendPathPacket(points);
|
||||
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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user