9
3

pathfinder_waypoint.cpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517
  1. #include <boost/geometry.hpp>
  2. #include <boost/geometry/geometries/point.hpp>
  3. #include <boost/geometry/index/rtree.hpp>
  4. #include <boost/graph/adjacency_list.hpp>
  5. #include <boost/graph/astar_search.hpp>
  6. #include <string>
  7. #include <memory>
  8. #include <iostream>
  9. #include <stdio.h>
  10. #include "pathfinder_waypoint.h"
  11. #include "../zoneserver.h"
  12. #include "../client.h"
  13. #pragma pack(1)
  14. struct NeighbourNode {
  15. int16 id;
  16. float distance;
  17. uint8 Teleport;
  18. int16 DoorID;
  19. };
  20. struct PathNode {
  21. uint16 id;
  22. glm::vec3 v;
  23. float bestz;
  24. NeighbourNode Neighbours[50];
  25. };
  26. struct PathFileHeader {
  27. uint32 version;
  28. uint32 PathNodeCount;
  29. };
  30. #pragma pack()
  31. struct Edge
  32. {
  33. float distance;
  34. bool teleport;
  35. int door_id;
  36. };
  37. struct Node
  38. {
  39. int id;
  40. glm::vec3 v;
  41. float bestz;
  42. std::map<int, Edge> edges;
  43. };
  44. template <class Graph, class CostType, class NodeMap>
  45. class distance_heuristic : public boost::astar_heuristic<Graph, CostType>
  46. {
  47. public:
  48. typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
  49. distance_heuristic(NodeMap n, Vertex goal)
  50. : m_node(n), m_goal(goal) {}
  51. CostType operator()(Vertex u)
  52. {
  53. CostType dx = m_node[m_goal].v.x - m_node[u].v.x;
  54. CostType dy = m_node[m_goal].v.y - m_node[u].v.y;
  55. CostType dz = m_node[m_goal].v.z - m_node[u].v.z;
  56. return ::sqrt(dx * dx + dy * dy + dz * dz);
  57. }
  58. private:
  59. NodeMap m_node;
  60. Vertex m_goal;
  61. };
  62. struct found_goal {};
  63. template <class Vertex>
  64. class astar_goal_visitor : public boost::default_astar_visitor
  65. {
  66. public:
  67. astar_goal_visitor(Vertex goal) : m_goal(goal) {}
  68. template <class Graph>
  69. void examine_vertex(Vertex u, Graph& g) {
  70. if (u == m_goal)
  71. throw found_goal();
  72. }
  73. private:
  74. Vertex m_goal;
  75. };
  76. typedef boost::geometry::model::point<float, 3, boost::geometry::cs::cartesian> Point;
  77. typedef std::pair<Point, unsigned int> RTreeValue;
  78. typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, boost::no_property,
  79. boost::property<boost::edge_weight_t, float>> GraphType;
  80. typedef boost::property_map<GraphType, boost::edge_weight_t>::type WeightMap;
  81. struct PathfinderWaypoint::Implementation {
  82. bool PathFileValid;
  83. boost::geometry::index::rtree<RTreeValue, boost::geometry::index::quadratic<16>> Tree;
  84. GraphType Graph;
  85. std::vector<Node> Nodes;
  86. std::string FileName;
  87. };
  88. PathfinderWaypoint::PathfinderWaypoint(const std::string &path)
  89. {
  90. m_impl.reset(new Implementation());
  91. m_impl->PathFileValid = false;
  92. m_impl->FileName = path;
  93. Load(path);
  94. }
  95. PathfinderWaypoint::~PathfinderWaypoint()
  96. {
  97. }
  98. IPathfinder::IPath PathfinderWaypoint::FindRoute(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, int flags)
  99. {
  100. stuck = false;
  101. partial = false;
  102. std::vector<RTreeValue> result_start_n;
  103. m_impl->Tree.query(boost::geometry::index::nearest(Point(start.x, start.y, start.z), 1), std::back_inserter(result_start_n));
  104. if (result_start_n.size() == 0) {
  105. return IPath();
  106. }
  107. std::vector<RTreeValue> result_end_n;
  108. m_impl->Tree.query(boost::geometry::index::nearest(Point(end.x, end.y, end.z), 1), std::back_inserter(result_end_n));
  109. if (result_end_n.size() == 0) {
  110. return IPath();
  111. }
  112. auto &nearest_start = *result_start_n.begin();
  113. auto &nearest_end = *result_end_n.begin();
  114. if (nearest_start.second == nearest_end.second) {
  115. IPath Route;
  116. Route.push_back(start);
  117. Route.push_back(end);
  118. return Route;
  119. }
  120. std::vector<GraphType::vertex_descriptor> p(boost::num_vertices(m_impl->Graph));
  121. try {
  122. boost::astar_search(m_impl->Graph, nearest_start.second,
  123. distance_heuristic<GraphType, float, Node*>(&m_impl->Nodes[0], nearest_end.second),
  124. boost::predecessor_map(&p[0])
  125. .visitor(astar_goal_visitor<size_t>(nearest_end.second)));
  126. }
  127. catch (found_goal)
  128. {
  129. IPath Route;
  130. Route.push_front(end);
  131. for (size_t v = nearest_end.second;; v = p[v]) {
  132. if (p[v] == v) {
  133. Route.push_front(m_impl->Nodes[v].v);
  134. break;
  135. }
  136. else {
  137. auto &node = m_impl->Nodes[v];
  138. auto iter = node.edges.find((int)p[v + 1]);
  139. if (iter != node.edges.end()) {
  140. auto &edge = iter->second;
  141. if (edge.teleport) {
  142. Route.push_front(m_impl->Nodes[v].v);
  143. Route.push_front(true);
  144. }
  145. else {
  146. Route.push_front(m_impl->Nodes[v].v);
  147. }
  148. }
  149. else {
  150. Route.push_front(m_impl->Nodes[v].v);
  151. }
  152. }
  153. }
  154. Route.push_front(start);
  155. return Route;
  156. }
  157. return IPath();
  158. }
  159. glm::vec3 PathfinderWaypoint::GetRandomLocation(const glm::vec3 &start)
  160. {
  161. if (m_impl->Nodes.size() > 0) {
  162. auto idx = MakeRandomInt(0, (int)m_impl->Nodes.size() - 1);// zone->random.Int(0, (int)m_impl->Nodes.size() - 1);
  163. auto &node = m_impl->Nodes[idx];
  164. return node.v;
  165. }
  166. return glm::vec3();
  167. }
  168. void PathfinderWaypoint::Load(const std::string &filename) {
  169. PathFileHeader Head;
  170. Head.PathNodeCount = 0;
  171. Head.version = 2;
  172. FILE *f = fopen(filename.c_str(), "rb");
  173. if (f) {
  174. char Magic[10];
  175. fread(&Magic, 9, 1, f);
  176. if (strncmp(Magic, "EQEMUPATH", 9))
  177. {
  178. //LogError("Bad Magic String in .path file");
  179. fclose(f);
  180. return;
  181. }
  182. fread(&Head, sizeof(Head), 1, f);
  183. /*LogInfo("Path File Header: Version [{}], PathNodes [{}]",
  184. (long)Head.version, (long)Head.PathNodeCount);
  185. */
  186. if (Head.version == 2)
  187. {
  188. LoadV2(f, Head);
  189. return;
  190. }
  191. else if (Head.version == 3) {
  192. LoadV3(f, Head);
  193. return;
  194. }
  195. else {
  196. //LogError("Unsupported path file version");
  197. fclose(f);
  198. return;
  199. }
  200. }
  201. }
  202. void PathfinderWaypoint::LoadV2(FILE *f, const PathFileHeader &header)
  203. {
  204. std::unique_ptr<PathNode[]> PathNodes(new PathNode[header.PathNodeCount]);
  205. fread(PathNodes.get(), sizeof(PathNode), header.PathNodeCount, f);
  206. int MaxNodeID = header.PathNodeCount - 1;
  207. m_impl->PathFileValid = true;
  208. m_impl->Nodes.reserve(header.PathNodeCount);
  209. for (uint32 i = 0; i < header.PathNodeCount; ++i)
  210. {
  211. auto &n = PathNodes[i];
  212. Node node;
  213. node.id = i;
  214. node.v = n.v;
  215. node.bestz = n.bestz;
  216. m_impl->Nodes.push_back(node);
  217. }
  218. auto weightmap = boost::get(boost::edge_weight, m_impl->Graph);
  219. for (uint32 i = 0; i < header.PathNodeCount; ++i) {
  220. for (uint32 j = 0; j < 50; ++j)
  221. {
  222. auto &node = m_impl->Nodes[i];
  223. if (PathNodes[i].Neighbours[j].id > MaxNodeID)
  224. {
  225. /*LogError("Path Node [{}], Neighbour [{}] ([{}]) out of range", i, j, PathNodes[i].Neighbours[j].id);
  226. m_impl->PathFileValid = false;*/
  227. }
  228. if (PathNodes[i].Neighbours[j].id > 0) {
  229. Edge edge;
  230. edge.distance = PathNodes[i].Neighbours[j].distance;
  231. edge.door_id = PathNodes[i].Neighbours[j].DoorID;
  232. edge.teleport = PathNodes[i].Neighbours[j].Teleport;
  233. node.edges[PathNodes[i].Neighbours[j].id] = edge;
  234. }
  235. }
  236. }
  237. BuildGraph();
  238. fclose(f);
  239. }
  240. void PathfinderWaypoint::LoadV3(FILE *f, const PathFileHeader &header)
  241. {
  242. m_impl->Nodes.reserve(header.PathNodeCount);
  243. uint32 edge_count = 0;
  244. fread(&edge_count, sizeof(uint32), 1, f);
  245. for (uint32 i = 0; i < header.PathNodeCount; ++i)
  246. {
  247. uint32 id = 0;
  248. float x = 0.0f;
  249. float y = 0.0f;
  250. float z = 0.0f;
  251. float best_z = 0.0f;
  252. fread(&id, sizeof(uint32), 1, f);
  253. fread(&x, sizeof(float), 1, f);
  254. fread(&y, sizeof(float), 1, f);
  255. fread(&z, sizeof(float), 1, f);
  256. fread(&best_z, sizeof(float), 1, f);
  257. Node n;
  258. n.id = id;
  259. n.bestz = best_z;
  260. n.v.x = x;
  261. n.v.y = y;
  262. n.v.z = z;
  263. m_impl->Nodes.push_back(n);
  264. }
  265. for (uint32 j = 0; j < edge_count; ++j) {
  266. uint32 from = 0;
  267. uint32 to = 0;
  268. int8 teleport = 0;
  269. float distance = 0.0f;
  270. int32 door_id = 0;
  271. fread(&from, sizeof(uint32), 1, f);
  272. fread(&to, sizeof(uint32), 1, f);
  273. fread(&teleport, sizeof(int8), 1, f);
  274. fread(&distance, sizeof(float), 1, f);
  275. fread(&door_id, sizeof(int32), 1, f);
  276. Edge e;
  277. e.teleport = teleport > 0 ? true : false;
  278. e.distance = distance;
  279. e.door_id = door_id;
  280. auto &n = m_impl->Nodes[from];
  281. n.edges[to] = e;
  282. }
  283. m_impl->PathFileValid = true;
  284. BuildGraph();
  285. fclose(f);
  286. }
  287. void PathfinderWaypoint::ShowNodes()
  288. {
  289. for (size_t i = 0; i < m_impl->Nodes.size(); ++i)
  290. {
  291. ShowNode(m_impl->Nodes[i]);
  292. }
  293. }
  294. void PathfinderWaypoint::ShowPath(Client *c, const glm::vec3 &start, const glm::vec3 &end)
  295. {
  296. bool partial = false;
  297. bool stuck = false;
  298. auto path = FindRoute(start, end, partial, stuck);
  299. std::vector<FindPerson_Point> points;
  300. FindPerson_Point p;
  301. for (auto &node : path)
  302. {
  303. if (!node.teleport) {
  304. p.x = node.pos.x;
  305. p.y = node.pos.y;
  306. p.z = node.pos.z;
  307. points.push_back(p);
  308. }
  309. }
  310. // c->SendPathPacket(points);
  311. }
  312. void PathfinderWaypoint::NodeInfo(Client *c)
  313. {
  314. if (!c->GetPlayer()->GetTarget()) {
  315. return;
  316. }
  317. auto node = FindPathNodeByCoordinates(c->GetPlayer()->GetTarget()->GetX(), c->GetPlayer()->GetTarget()->GetZ(), c->GetPlayer()->GetTarget()->GetY());
  318. if (node == nullptr) {
  319. return;
  320. }
  321. /* c->Message(Chat::White, "Pathing node: %i at (%.2f, %.2f, %.2f) with bestz %.2f",
  322. node->id, node->v.x, node->v.y, node->v.z, node->bestz);
  323. for (auto &edge : node->edges) {
  324. c->Message(Chat::White, "id: %i, distance: %.2f, door id: %i, is teleport: %i",
  325. edge.first,
  326. edge.second.distance,
  327. edge.second.door_id,
  328. edge.second.teleport);
  329. }*/
  330. }
  331. Node *PathfinderWaypoint::FindPathNodeByCoordinates(float x, float y, float z)
  332. {
  333. for (auto &node : m_impl->Nodes) {
  334. auto dist = Distance(glm::vec3(x, y, z), node.v);
  335. if (dist < 0.1) {
  336. return &node;
  337. }
  338. }
  339. return nullptr;
  340. }
  341. void PathfinderWaypoint::BuildGraph()
  342. {
  343. m_impl->Graph = GraphType();
  344. m_impl->Tree = boost::geometry::index::rtree<RTreeValue, boost::geometry::index::quadratic<16>>();
  345. for (auto &node : m_impl->Nodes) {
  346. RTreeValue rtv;
  347. rtv.first = Point(node.v.x, node.v.y, node.v.z);
  348. rtv.second = node.id;
  349. m_impl->Tree.insert(rtv);
  350. boost::add_vertex(m_impl->Graph);
  351. }
  352. //Populate edges now that we've created all the nodes
  353. auto weightmap = boost::get(boost::edge_weight, m_impl->Graph);
  354. for (auto &node : m_impl->Nodes) {
  355. for (auto &edge : node.edges) {
  356. GraphType::edge_descriptor e;
  357. bool inserted;
  358. boost::tie(e, inserted) = boost::add_edge(node.id, edge.first, m_impl->Graph);
  359. weightmap[e] = edge.second.distance;
  360. }
  361. }
  362. }
  363. std::string DigitToWord(int i)
  364. {
  365. std::string digit = std::to_string(i);
  366. std::string ret;
  367. for (size_t idx = 0; idx < digit.length(); ++idx) {
  368. if (!ret.empty()) {
  369. ret += "_";
  370. }
  371. switch (digit[idx]) {
  372. case '0':
  373. ret += "Zero";
  374. break;
  375. case '1':
  376. ret += "One";
  377. break;
  378. case '2':
  379. ret += "Two";
  380. break;
  381. case '3':
  382. ret += "Three";
  383. break;
  384. case '4':
  385. ret += "Four";
  386. break;
  387. case '5':
  388. ret += "Five";
  389. break;
  390. case '6':
  391. ret += "Six";
  392. break;
  393. case '7':
  394. ret += "Seven";
  395. break;
  396. case '8':
  397. ret += "Eight";
  398. break;
  399. case '9':
  400. ret += "Nine";
  401. break;
  402. default:
  403. break;
  404. }
  405. }
  406. return ret;
  407. }
  408. void PathfinderWaypoint::ShowNode(const Node &n) {
  409. /* auto npc_type = new NPCType;
  410. memset(npc_type, 0, sizeof(NPCType));
  411. sprintf(npc_type->name, "%s", DigitToWord(n.id).c_str());
  412. sprintf(npc_type->lastname, "%i", n.id);
  413. npc_type->current_hp = 4000000;
  414. npc_type->max_hp = 4000000;
  415. npc_type->race = 2254;
  416. npc_type->gender = 2;
  417. npc_type->class_ = 9;
  418. npc_type->deity = 1;
  419. npc_type->level = 75;
  420. npc_type->npc_id = 0;
  421. npc_type->loottable_id = 0;
  422. npc_type->texture = 1;
  423. npc_type->light = 0;
  424. npc_type->runspeed = 0;
  425. npc_type->d_melee_texture1 = 1;
  426. npc_type->d_melee_texture2 = 1;
  427. npc_type->merchanttype = 1;
  428. npc_type->bodytype = 1;
  429. npc_type->show_name = true;
  430. npc_type->STR = 150;
  431. npc_type->STA = 150;
  432. npc_type->DEX = 150;
  433. npc_type->AGI = 150;
  434. npc_type->INT = 150;
  435. npc_type->WIS = 150;
  436. npc_type->CHA = 150;
  437. npc_type->findable = 1;
  438. auto position = glm::vec4(n.v.x, n.v.y, n.v.z, 0.0f);
  439. auto npc = new NPC(npc_type, nullptr, position, GravityBehavior::Flying);
  440. npc->GiveNPCTypeData(npc_type);
  441. entity_list.AddNPC(npc, true, true);*/
  442. }