From c2766db89da1cf0eb82dd6dd20f0e438ee3bb590 Mon Sep 17 00:00:00 2001 From: KimLS Date: Wed, 19 Jul 2017 19:54:26 -0700 Subject: [PATCH] Working on waypoint code, using boost graph libs --- zone/command.cpp | 4 + zone/pathfinder_interface.cpp | 17 +- zone/pathfinder_interface.h | 4 + zone/pathfinder_nav_mesh.h | 13 +- zone/pathfinder_null.h | 1 + zone/pathfinder_waypoint.cpp | 372 ++++++++++++++++++++++++++++++++++ zone/pathfinder_waypoint.h | 13 +- zone/pathing.cpp | 28 ++- 8 files changed, 445 insertions(+), 7 deletions(-) diff --git a/zone/command.cpp b/zone/command.cpp index c11500ffc..405eb8889 100644 --- a/zone/command.cpp +++ b/zone/command.cpp @@ -6813,6 +6813,10 @@ void command_qglobal(Client *c, const Seperator *sep) { void command_path(Client *c, const Seperator *sep) { + if (zone->pathing) { + zone->pathing->DebugCommand(c, sep); + } + //if(sep->arg[1][0] == '\0' || !strcasecmp(sep->arg[1], "help")) //{ // c->Message(0, "Syntax: #path shownodes: Spawns a npc to represent every npc node."); diff --git a/zone/pathfinder_interface.cpp b/zone/pathfinder_interface.cpp index f3110808e..2a60ada74 100644 --- a/zone/pathfinder_interface.cpp +++ b/zone/pathfinder_interface.cpp @@ -1,7 +1,22 @@ +#include "../common/seperator.h" +#include "client.h" #include "pathfinder_null.h" #include "pathfinder_nav_mesh.h" #include "pathfinder_waypoint.h" +#include +#include IPathfinder *IPathfinder::Load(const std::string &zone) { + struct stat statbuffer; + std::string waypoint_path = fmt::format("maps/{0}.path", zone); + std::string navmesh_path = fmt::format("maps/{0}.nav", zone); + if (stat(waypoint_path.c_str(), &statbuffer) == 0) { + return new PathfinderWaypoint(waypoint_path); + } + + //if (stat(waypoint_path.c_str(), &statbuffer) == 0) { + // return new PathfinderNavmesh(navmesh_path); + //} + return new PathfinderNull(); -} \ No newline at end of file +} diff --git a/zone/pathfinder_interface.h b/zone/pathfinder_interface.h index ea4aa6642..e701b7f37 100644 --- a/zone/pathfinder_interface.h +++ b/zone/pathfinder_interface.h @@ -3,6 +3,9 @@ #include "map.h" #include +class Client; +class Seperator; + class IPathfinder { public: @@ -13,6 +16,7 @@ public: virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end) = 0; virtual glm::vec3 GetRandomLocation() = 0; + virtual void DebugCommand(Client *c, const Seperator *sep) = 0; static IPathfinder *Load(const std::string &zone); }; diff --git a/zone/pathfinder_nav_mesh.h b/zone/pathfinder_nav_mesh.h index bdb83a41e..bec5c1b44 100644 --- a/zone/pathfinder_nav_mesh.h +++ b/zone/pathfinder_nav_mesh.h @@ -1,3 +1,14 @@ #pragma once -#include "pathfinder_interface.h" \ No newline at end of file +#include "pathfinder_interface.h" + +class PathfinderNavmesh : public IPathfinder +{ +public: + PathfinderNavmesh() { } + virtual ~PathfinderNavmesh() { } + + virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end); + virtual glm::vec3 GetRandomLocation(); + virtual void DebugCommand(Client *c, const Seperator *sep); +}; \ No newline at end of file diff --git a/zone/pathfinder_null.h b/zone/pathfinder_null.h index 6d2674230..55ff2b06b 100644 --- a/zone/pathfinder_null.h +++ b/zone/pathfinder_null.h @@ -10,4 +10,5 @@ public: virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end); virtual glm::vec3 GetRandomLocation(); + virtual void DebugCommand(Client *c, const Seperator *sep) { } }; \ No newline at end of file diff --git a/zone/pathfinder_waypoint.cpp b/zone/pathfinder_waypoint.cpp index e69de29bb..d6e88353e 100644 --- a/zone/pathfinder_waypoint.cpp +++ b/zone/pathfinder_waypoint.cpp @@ -0,0 +1,372 @@ +#include +#include +#include +#include +#include + +#include "pathfinder_waypoint.h" +#include "zone.h" +#include "client.h" +#include "../common/eqemu_logsys.h" +#include "../common/rulesys.h" +#include +#include + +extern Zone *zone; + +#pragma pack(1) +struct NeighbourNode { + int16 id; + float distance; + uint8 Teleport; + int16 DoorID; +}; + +struct PathNode { + uint16 id; + glm::vec3 v; + float bestz; + NeighbourNode Neighbours[50]; +}; + +struct PathFileHeader { + uint32 version; + uint32 PathNodeCount; +}; +#pragma pack() + +struct Node +{ + glm::vec3 v; + float bestz; +}; + +struct Edge +{ + float distance; + bool teleport; + int door_id; +}; +template +class distance_heuristic : public boost::astar_heuristic +{ +public: + typedef typename boost::graph_traits::vertex_descriptor Vertex; + + distance_heuristic(NodeMap n, Vertex goal) + : m_node(n), m_goal(goal) {} + CostType operator()(Vertex u) + { + CostType dx = m_node[m_goal].v.x - m_node[u].v.x; + CostType dy = m_node[m_goal].v.y - m_node[u].v.y; + CostType dz = m_node[m_goal].v.z - m_node[u].v.z; + return ::sqrt(dx * dx + dy * dy + dz * dz); + } +private: + NodeMap m_node; + Vertex m_goal; +}; + +struct found_goal {}; +template +class astar_goal_visitor : public boost::default_astar_visitor +{ +public: + astar_goal_visitor(Vertex goal) : m_goal(goal) {} + template + void examine_vertex(Vertex u, Graph& g) { + if (u == m_goal) + throw found_goal(); + } +private: + Vertex m_goal; +}; + +typedef boost::geometry::model::point Point; +typedef boost::geometry::model::box Box; +typedef std::pair RTreeValue; +typedef boost::adjacency_list> GraphType; +typedef boost::property_map::type WeightMap; + +struct PathfinderWaypoint::Implementation { + bool PathFileValid; + boost::geometry::index::rtree> Tree; + GraphType Graph; + std::vector Nodes; + std::vector Edges; +}; + +PathfinderWaypoint::PathfinderWaypoint(const std::string &path) +{ + PathFileHeader Head; + m_impl.reset(new Implementation()); + m_impl->PathFileValid = false; + Head.PathNodeCount = 0; + Head.version = 2; + + FILE *f = fopen(path.c_str(), "rb"); + if (f) { + char Magic[10]; + + fread(&Magic, 9, 1, f); + + if (strncmp(Magic, "EQEMUPATH", 9)) + { + Log(Logs::General, Logs::Error, "Bad Magic String in .path file."); + return; + } + + fread(&Head, sizeof(Head), 1, f); + + Log(Logs::General, Logs::Status, "Path File Header: Version %ld, PathNodes %ld", + (long)Head.version, (long)Head.PathNodeCount); + + if (Head.version != 2) + { + Log(Logs::General, Logs::Error, "Unsupported path file version."); + return; + } + + std::unique_ptr PathNodes(new PathNode[Head.PathNodeCount]); + + fread(PathNodes.get(), sizeof(PathNode), Head.PathNodeCount, f); + + int MaxNodeID = Head.PathNodeCount - 1; + + m_impl->PathFileValid = true; + + m_impl->Nodes.reserve(Head.PathNodeCount); + for (uint32 i = 0; i < Head.PathNodeCount; ++i) + { + auto &n = PathNodes[i]; + + Point p = Point(n.v.x, n.v.y, n.v.z); + m_impl->Tree.insert(std::make_pair(p, i)); + + boost::add_vertex(m_impl->Graph); + Node node; + node.v = n.v; + node.bestz = n.bestz; + m_impl->Nodes.push_back(node); + } + + auto weightmap = boost::get(boost::edge_weight, m_impl->Graph); + for (uint32 i = 0; i < Head.PathNodeCount; ++i) { + for (uint32 j = 0; j < 50; ++j) + { + if (PathNodes[i].Neighbours[j].id > MaxNodeID) + { + Log(Logs::General, Logs::Error, "Path Node %i, Neighbour %i (%i) out of range.", i, j, PathNodes[i].Neighbours[j].id); + m_impl->PathFileValid = false; + } + + if (PathNodes[i].Neighbours[j].id > 0) { + GraphType::edge_descriptor e; + bool inserted; + boost::tie(e, inserted) = boost::add_edge(PathNodes[i].id, PathNodes[i].Neighbours[j].id, m_impl->Graph); + weightmap[e] = PathNodes[i].Neighbours[j].distance; + + Edge edge; + edge.distance = PathNodes[i].Neighbours[j].distance; + edge.door_id = PathNodes[i].Neighbours[j].DoorID; + edge.teleport = PathNodes[i].Neighbours[j].Teleport; + m_impl->Edges.push_back(edge); + } + } + } + + fclose(f); + } +} + +PathfinderWaypoint::~PathfinderWaypoint() +{ +} + +IPathfinder::IPath PathfinderWaypoint::FindRoute(const glm::vec3 &start, const glm::vec3 &end) +{ + std::vector result_start_n; + m_impl->Tree.query(boost::geometry::index::nearest(Point(start.x, start.y, start.z), 1), std::back_inserter(result_start_n)); + if (result_start_n.size() == 0) { + IPath Route; + Route.push_back(start); + Route.push_back(end); + return Route; + } + + std::vector result_end_n; + m_impl->Tree.query(boost::geometry::index::nearest(Point(end.x, end.y, end.z), 1), std::back_inserter(result_end_n)); + if (result_end_n.size() == 0) { + IPath Route; + Route.push_back(start); + Route.push_back(end); + return Route; + } + + auto &nearest_start = *result_start_n.begin(); + auto &nearest_end = *result_end_n.begin(); + + Log(Logs::General, Logs::Status, "Nearest start point found to be (%f, %f, %f) with node %u", nearest_start.first.get<0>(), nearest_start.first.get<1>(), nearest_start.first.get<2>(), nearest_start.second); + Log(Logs::General, Logs::Status, "Nearest end point found to be (%f, %f, %f) with node %u", nearest_end.first.get<0>(), nearest_end.first.get<1>(), nearest_end.first.get<2>(), nearest_end.second); + + std::vector p(boost::num_vertices(m_impl->Graph)); + std::vector d(boost::num_vertices(m_impl->Graph)); + try { + boost::astar_search(m_impl->Graph, nearest_start.second, + distance_heuristic(&m_impl->Nodes[0], nearest_end.second), + boost::predecessor_map(&p[0]) + .distance_map(&d[0]) + .visitor(astar_goal_visitor(nearest_end.second))); + } + catch (found_goal) + { + IPath Route; + + Route.push_front(end); + for (size_t v = nearest_end.second;; v = p[v]) { + Route.push_front(m_impl->Nodes[v].v); + if (p[v] == v) + break; + } + Route.push_front(start); + + return Route; + } + + + IPath Route; + Route.push_back(start); + Route.push_back(end); + return Route; +} + +glm::vec3 PathfinderWaypoint::GetRandomLocation() +{ + return glm::vec3(); +} + +void PathfinderWaypoint::DebugCommand(Client *c, const Seperator *sep) +{ + if(sep->arg[1][0] == '\0' || !strcasecmp(sep->arg[1], "help")) + { + c->Message(0, "Syntax: #path shownodes: Spawns a npc to represent every npc node."); + c->Message(0, "#path info node_id: Gives information about node info (requires shownode target)."); + c->Message(0, "#path dump file_name: Dumps the current zone->pathing to a file of your naming."); + c->Message(0, "#path add [requested_id]: Adds a node at your current location will try to take the requested id if possible."); + c->Message(0, "#path connect connect_to_id [is_teleport] [door_id]: Connects the currently targeted node to connect_to_id's node and connects that node back (requires shownode target)."); + c->Message(0, "#path sconnect connect_to_id [is_teleport] [door_id]: Connects the currently targeted node to connect_to_id's node (requires shownode target)."); + c->Message(0, "#path qconnect [set]: short cut connect, connects the targeted node to the node you set with #path qconnect set (requires shownode target)."); + c->Message(0, "#path disconnect [all]/disconnect_from_id: Disconnects the currently targeted node to disconnect from disconnect from id's node (requires shownode target), if passed all as the second argument it will disconnect this node from every other node."); + c->Message(0, "#path move: Moves your targeted node to your current position"); + c->Message(0, "#path process file_name: processes the map file and tries to automatically generate a rudimentary path setup and then dumps the current zone->pathing to a file of your naming."); + c->Message(0, "#path resort [nodes]: resorts the connections/nodes after you've manually altered them so they'll work."); + return; + } + + if(!strcasecmp(sep->arg[1], "shownodes")) + { + ShowNodes(); + return; + } + + if (!strcasecmp(sep->arg[1], "show")) + { + if (c->GetTarget() != nullptr) { + auto target = c->GetTarget(); + glm::vec3 start(target->GetX(), target->GetY(), target->GetZ()); + glm::vec3 end(c->GetX(), c->GetY(), c->GetZ()); + + ShowPath(start, end); + } + + return; + } +} + +void PathfinderWaypoint::ShowNodes() +{ + for (size_t i = 0; i < m_impl->Nodes.size(); ++i) + { + auto npc_type = new NPCType; + memset(npc_type, 0, sizeof(NPCType)); + + auto c = i / 1000u; + sprintf(npc_type->name, "Node%u", c); + npc_type->cur_hp = 4000000; + npc_type->max_hp = 4000000; + npc_type->race = 2254; + npc_type->gender = 2; + npc_type->class_ = 9; + npc_type->deity = 1; + npc_type->level = 75; + npc_type->npc_id = 0; + npc_type->loottable_id = 0; + npc_type->texture = 1; + npc_type->light = 0; + npc_type->runspeed = 0; + npc_type->d_melee_texture1 = 1; + npc_type->d_melee_texture2 = 1; + npc_type->merchanttype = 1; + npc_type->bodytype = 1; + + npc_type->STR = 150; + npc_type->STA = 150; + npc_type->DEX = 150; + npc_type->AGI = 150; + npc_type->INT = 150; + npc_type->WIS = 150; + npc_type->CHA = 150; + + npc_type->findable = 1; + auto position = glm::vec4(m_impl->Nodes[i].v.x, m_impl->Nodes[i].v.y, m_impl->Nodes[i].v.z, 0.0f); + auto npc = new NPC(npc_type, nullptr, position, FlyMode1); + npc->GiveNPCTypeData(npc_type); + + entity_list.AddNPC(npc, true, true); + } +} + +void PathfinderWaypoint::ShowPath(const glm::vec3 &start, const glm::vec3 &end) +{ + auto path = FindRoute(start, end); + + for (auto &node : path) + { + auto npc_type = new NPCType; + memset(npc_type, 0, sizeof(NPCType)); + + sprintf(npc_type->name, "Path"); + npc_type->cur_hp = 4000000; + npc_type->max_hp = 4000000; + npc_type->race = 2254; + npc_type->gender = 2; + npc_type->class_ = 9; + npc_type->deity = 1; + npc_type->level = 75; + npc_type->npc_id = 0; + npc_type->loottable_id = 0; + npc_type->texture = 1; + npc_type->light = 0; + npc_type->runspeed = 0; + npc_type->d_melee_texture1 = 1; + npc_type->d_melee_texture2 = 1; + npc_type->merchanttype = 1; + npc_type->bodytype = 1; + + npc_type->STR = 150; + npc_type->STA = 150; + npc_type->DEX = 150; + npc_type->AGI = 150; + npc_type->INT = 150; + npc_type->WIS = 150; + npc_type->CHA = 150; + + npc_type->findable = 1; + auto position = glm::vec4(node.x, node.y, node.z, 0.0f); + auto npc = new NPC(npc_type, nullptr, position, FlyMode1); + npc->GiveNPCTypeData(npc_type); + + entity_list.AddNPC(npc, true, true); + } +} diff --git a/zone/pathfinder_waypoint.h b/zone/pathfinder_waypoint.h index 5c51ac915..493531802 100644 --- a/zone/pathfinder_waypoint.h +++ b/zone/pathfinder_waypoint.h @@ -1,13 +1,22 @@ #pragma once #include "pathfinder_interface.h" +#include class PathfinderWaypoint : public IPathfinder { public: - PathfinderWaypoint() { } - virtual ~PathfinderWaypoint() { } + PathfinderWaypoint(const std::string &path); + virtual ~PathfinderWaypoint(); virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end); virtual glm::vec3 GetRandomLocation(); + virtual void DebugCommand(Client *c, const Seperator *sep); + +private: + void ShowNodes(); + void ShowPath(const glm::vec3 &start, const glm::vec3 &end); + + struct Implementation; + std::unique_ptr m_impl; }; \ No newline at end of file diff --git a/zone/pathing.cpp b/zone/pathing.cpp index 599e8af6b..8d6c677b7 100644 --- a/zone/pathing.cpp +++ b/zone/pathing.cpp @@ -666,7 +666,7 @@ glm::vec3 Mob::UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &Wa return *Route.begin(); } else { - bool SameDestination = DistanceSquared(To, PathingDestination) < 1.0f; + bool SameDestination = DistanceSquared(To, PathingDestination) < 4.0f; if (!SameDestination) { //We had a route but our target position moved too much Route = zone->pathing->FindRoute(From, To); @@ -676,7 +676,7 @@ glm::vec3 Mob::UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &Wa return *Route.begin(); } else { - bool AtNextNode = DistanceSquared(From, *Route.begin()) < 1.0f; + bool AtNextNode = DistanceSquared(From, *Route.begin()) < 4.0f; if (AtNextNode) { WaypointChanged = false; NodeReached = true; @@ -690,7 +690,29 @@ glm::vec3 Mob::UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &Wa return *Route.begin(); } else { - return *Route.begin(); + auto node = *Route.begin(); + if (node.x == 1000000.0f && node.y == 1000000.0f && node.z == 1000000.0f) { + //If is identity node then is teleport node. + Route.pop_front(); + + if (Route.empty()) { + return To; + } + + auto nextNode = *Route.begin(); + + Teleport(nextNode); + + Route.pop_front(); + + if (Route.empty()) { + return To; + } + + return *Route.begin(); + } + + return node; } } else {