pathfinder_nav_mesh.cpp 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514
  1. #include <memory>
  2. #include <stdio.h>
  3. #include <vector>
  4. #include "../../common/Log.h"
  5. #include "pathfinder_nav_mesh.h"
  6. #include <DetourCommon.h>
  7. #include <DetourNavMeshQuery.h>
  8. #include "../zoneserver.h"
  9. #include "water_map.h"
  10. #include "../client.h"
  11. struct PathfinderNavmesh::Implementation
  12. {
  13. dtNavMesh *nav_mesh;
  14. dtNavMeshQuery *query;
  15. };
  16. PathfinderNavmesh::PathfinderNavmesh(const std::string &path)
  17. {
  18. m_impl.reset(new Implementation());
  19. m_impl->nav_mesh = nullptr;
  20. m_impl->query = nullptr;
  21. Load(path);
  22. }
  23. PathfinderNavmesh::~PathfinderNavmesh()
  24. {
  25. Clear();
  26. }
  27. IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, int flags)
  28. {
  29. partial = false;
  30. if (!m_impl->nav_mesh) {
  31. return IPath();
  32. }
  33. if (!m_impl->query) {
  34. m_impl->query = dtAllocNavMeshQuery();
  35. }
  36. m_impl->query->init(m_impl->nav_mesh, 4092 /*RuleI(Pathing, MaxNavmeshNodes)*/);
  37. glm::vec3 current_location(start.x, start.z, start.y);
  38. glm::vec3 dest_location(end.x, end.z, end.y);
  39. dtQueryFilter filter;
  40. filter.setIncludeFlags(flags);
  41. filter.setAreaCost(0, 1.0f); //Normal
  42. filter.setAreaCost(1, 3.0f); //Water
  43. filter.setAreaCost(2, 5.0f); //Lava
  44. filter.setAreaCost(4, 1.0f); //PvP
  45. filter.setAreaCost(5, 2.0f); //Slime
  46. filter.setAreaCost(6, 2.0f); //Ice
  47. filter.setAreaCost(7, 4.0f); //V Water (Frigid Water)
  48. filter.setAreaCost(8, 1.0f); //General Area
  49. filter.setAreaCost(9, 0.1f); //Portal
  50. filter.setAreaCost(10, 0.1f); //Prefer
  51. dtPolyRef start_ref;
  52. dtPolyRef end_ref;
  53. glm::vec3 ext(5.0f, 100.0f, 5.0f);
  54. m_impl->query->findNearestPoly(&current_location[0], &ext[0], &filter, &start_ref, 0);
  55. m_impl->query->findNearestPoly(&dest_location[0], &ext[0], &filter, &end_ref, 0);
  56. if (!start_ref || !end_ref) {
  57. return IPath();
  58. }
  59. int npoly = 0;
  60. dtPolyRef path[1024] = { 0 };
  61. auto status = m_impl->query->findPath(start_ref, end_ref, &current_location[0], &dest_location[0], &filter, path, &npoly, 1024);
  62. if (npoly) {
  63. glm::vec3 epos = dest_location;
  64. if (path[npoly - 1] != end_ref) {
  65. m_impl->query->closestPointOnPoly(path[npoly - 1], &dest_location[0], &epos[0], 0);
  66. partial = true;
  67. auto dist = DistanceSquared(epos, current_location);
  68. if (dist < 10000.0f) {
  69. stuck = true;
  70. }
  71. }
  72. float straight_path[2048 * 3];
  73. unsigned char straight_path_flags[2048];
  74. int n_straight_polys;
  75. dtPolyRef straight_path_polys[2048];
  76. status = m_impl->query->findStraightPath(&current_location[0], &epos[0], path, npoly,
  77. straight_path, straight_path_flags,
  78. straight_path_polys, &n_straight_polys, 2048, DT_STRAIGHTPATH_AREA_CROSSINGS);
  79. if (dtStatusFailed(status)) {
  80. return IPath();
  81. }
  82. if (n_straight_polys) {
  83. IPath Route;
  84. for (int i = 0; i < n_straight_polys; ++i)
  85. {
  86. glm::vec3 node;
  87. node.x = straight_path[i * 3];
  88. node.z = straight_path[i * 3 + 1];
  89. node.y = straight_path[i * 3 + 2];
  90. Route.push_back(node);
  91. unsigned short flag = 0;
  92. if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
  93. if (flag & 512) {
  94. Route.push_back(true);
  95. }
  96. }
  97. }
  98. return Route;
  99. }
  100. }
  101. IPath Route;
  102. Route.push_back(end);
  103. return Route;
  104. }
  105. IPathfinder::IPath PathfinderNavmesh::FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions &opts)
  106. {
  107. partial = false;
  108. if (!m_impl->nav_mesh) {
  109. return IPath();
  110. }
  111. if (!m_impl->query) {
  112. m_impl->query = dtAllocNavMeshQuery();
  113. }
  114. m_impl->query->init(m_impl->nav_mesh, 4092 /*RuleI(Pathing, MaxNavmeshNodes)*/);
  115. glm::vec3 current_location(start.x, start.z, start.y);
  116. glm::vec3 dest_location(end.x, end.z, end.y);
  117. dtQueryFilter filter;
  118. filter.setIncludeFlags(opts.flags);
  119. filter.setAreaCost(0, opts.flag_cost[0]); //Normal
  120. filter.setAreaCost(1, opts.flag_cost[1]); //Water
  121. filter.setAreaCost(2, opts.flag_cost[2]); //Lava
  122. filter.setAreaCost(4, opts.flag_cost[3]); //PvP
  123. filter.setAreaCost(5, opts.flag_cost[4]); //Slime
  124. filter.setAreaCost(6, opts.flag_cost[5]); //Ice
  125. filter.setAreaCost(7, opts.flag_cost[6]); //V Water (Frigid Water)
  126. filter.setAreaCost(8, opts.flag_cost[7]); //General Area
  127. filter.setAreaCost(9, opts.flag_cost[8]); //Portal
  128. filter.setAreaCost(10, opts.flag_cost[9]); //Prefer
  129. static const int max_polys = 256;
  130. dtPolyRef start_ref;
  131. dtPolyRef end_ref;
  132. glm::vec3 ext(10.0f, 200.0f, 10.0f);
  133. m_impl->query->findNearestPoly(&current_location[0], &ext[0], &filter, &start_ref, 0);
  134. m_impl->query->findNearestPoly(&dest_location[0], &ext[0], &filter, &end_ref, 0);
  135. if (!start_ref || !end_ref) {
  136. return IPath();
  137. }
  138. int npoly = 0;
  139. dtPolyRef path[max_polys] = { 0 };
  140. auto status = m_impl->query->findPath(start_ref, end_ref, &current_location[0], &dest_location[0], &filter, path, &npoly, max_polys);
  141. if (npoly) {
  142. glm::vec3 epos = dest_location;
  143. if (path[npoly - 1] != end_ref) {
  144. m_impl->query->closestPointOnPoly(path[npoly - 1], &dest_location[0], &epos[0], 0);
  145. partial = true;
  146. auto dist = DistanceSquared(epos, current_location);
  147. if (dist < 10000.0f) {
  148. stuck = true;
  149. }
  150. }
  151. int n_straight_polys;
  152. glm::vec3 straight_path[max_polys];
  153. unsigned char straight_path_flags[max_polys];
  154. dtPolyRef straight_path_polys[max_polys];
  155. auto status = m_impl->query->findStraightPath(&current_location[0], &epos[0], path, npoly,
  156. (float*)&straight_path[0], straight_path_flags,
  157. straight_path_polys, &n_straight_polys, 2048, DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS);
  158. if (dtStatusFailed(status)) {
  159. return IPath();
  160. }
  161. if (n_straight_polys) {
  162. if (opts.smooth_path) {
  163. IPath Route;
  164. //Add the first point
  165. {
  166. auto &flag = straight_path_flags[0];
  167. if (flag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
  168. auto &p = straight_path[0];
  169. Route.push_back(glm::vec3(p.x, p.z, p.y));
  170. }
  171. else {
  172. auto &p = straight_path[0];
  173. float h = 0.0f;
  174. if (dtStatusSucceed(GetPolyHeightOnPath(path, npoly, p, &h))) {
  175. p.y = h + opts.offset;
  176. }
  177. Route.push_back(glm::vec3(p.x, p.z, p.y));
  178. }
  179. }
  180. for (int i = 0; i < n_straight_polys - 1; ++i)
  181. {
  182. auto &flag = straight_path_flags[i];
  183. if (flag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
  184. auto &poly = straight_path_polys[i];
  185. auto &p2 = straight_path[i + 1];
  186. glm::vec3 node(p2.x, p2.z, p2.y);
  187. Route.push_back(node);
  188. unsigned short pflag = 0;
  189. if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &pflag))) {
  190. if (pflag & 512) {
  191. Route.push_back(true);
  192. }
  193. }
  194. }
  195. else {
  196. auto &p1 = straight_path[i];
  197. auto &p2 = straight_path[i + 1];
  198. auto dist = glm::distance(p1, p2);
  199. auto dir = glm::normalize(p2 - p1);
  200. float total = 0.0f;
  201. glm::vec3 previous_pt = p1;
  202. while (total < dist) {
  203. glm::vec3 current_pt;
  204. float dist_to_move = opts.step_size;
  205. float ff = opts.step_size / 2.0f;
  206. if (total + dist_to_move + ff >= dist) {
  207. current_pt = p2;
  208. total = dist;
  209. }
  210. else {
  211. total += dist_to_move;
  212. current_pt = p1 + dir * total;
  213. }
  214. float h = 0.0f;
  215. if (dtStatusSucceed(GetPolyHeightOnPath(path, npoly, current_pt, &h))) {
  216. current_pt.y = h + opts.offset;
  217. }
  218. Route.push_back(glm::vec3(current_pt.x, current_pt.z, current_pt.y));
  219. previous_pt = current_pt;
  220. }
  221. }
  222. }
  223. return Route;
  224. }
  225. else {
  226. IPath Route;
  227. for (int i = 0; i < n_straight_polys; ++i)
  228. {
  229. auto &current = straight_path[i];
  230. glm::vec3 node(current.x, current.z, current.y);
  231. Route.push_back(node);
  232. unsigned short flag = 0;
  233. if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
  234. if (flag & 512) {
  235. Route.push_back(true);
  236. }
  237. }
  238. }
  239. return Route;
  240. }
  241. }
  242. }
  243. return IPath();
  244. }
  245. glm::vec3 PathfinderNavmesh::GetRandomLocation(const glm::vec3 &start)
  246. {
  247. if (start.x == 0.0f && start.y == 0.0)
  248. return glm::vec3(0.f);
  249. if (!m_impl->nav_mesh) {
  250. return glm::vec3(0.f);
  251. }
  252. if (!m_impl->query) {
  253. m_impl->query = dtAllocNavMeshQuery();
  254. m_impl->query->init(m_impl->nav_mesh, 4092 /*RuleI(Pathing, MaxNavmeshNodes)*/);
  255. }
  256. dtQueryFilter filter;
  257. filter.setIncludeFlags(65535U ^ 2048);
  258. filter.setAreaCost(0, 1.0f); //Normal
  259. filter.setAreaCost(1, 3.0f); //Water
  260. filter.setAreaCost(2, 5.0f); //Lava
  261. filter.setAreaCost(4, 1.0f); //PvP
  262. filter.setAreaCost(5, 2.0f); //Slime
  263. filter.setAreaCost(6, 2.0f); //Ice
  264. filter.setAreaCost(7, 4.0f); //V Water (Frigid Water)
  265. filter.setAreaCost(8, 1.0f); //General Area
  266. filter.setAreaCost(9, 0.1f); //Portal
  267. filter.setAreaCost(10, 0.1f); //Prefer
  268. dtPolyRef randomRef;
  269. float point[3];
  270. dtPolyRef start_ref;
  271. glm::vec3 current_location(start.x, start.z, start.y);
  272. glm::vec3 ext(5.0f, 100.0f, 5.0f);
  273. m_impl->query->findNearestPoly(&current_location[0], &ext[0], &filter, &start_ref, 0);
  274. if (!start_ref)
  275. {
  276. return glm::vec3(0.f);
  277. }
  278. if (dtStatusSucceed(m_impl->query->findRandomPointAroundCircle(start_ref, &current_location[0], 100.f, &filter, []() { return MakeRandomFloat(0.0,1.0); /*(float)zone->random.Real(0.0, 1.0);*/ }, &randomRef, point)))
  279. {
  280. return glm::vec3(point[0], point[2], point[1]);
  281. }
  282. return glm::vec3(0.f);
  283. }
  284. void PathfinderNavmesh::Clear()
  285. {
  286. if (m_impl->nav_mesh) {
  287. dtFreeNavMesh(m_impl->nav_mesh);
  288. }
  289. if (m_impl->query) {
  290. dtFreeNavMeshQuery(m_impl->query);
  291. }
  292. }
  293. void PathfinderNavmesh::Load(const std::string &path)
  294. {
  295. Clear();
  296. FILE *f = fopen(path.c_str(), "rb");
  297. if (f) {
  298. NavMeshSetHeader header;
  299. size_t readLen = fread(&header, sizeof(NavMeshSetHeader), 1, f);
  300. if (readLen != 1)
  301. {
  302. fclose(f);
  303. return;
  304. }
  305. if (header.magic != NAVMESHSET_MAGIC)
  306. {
  307. fclose(f);
  308. return;
  309. }
  310. if (header.version != NAVMESHSET_VERSION)
  311. {
  312. fclose(f);
  313. return;
  314. }
  315. dtNavMesh* mesh = dtAllocNavMesh();
  316. if (!mesh)
  317. {
  318. fclose(f);
  319. return;
  320. }
  321. dtStatus status = mesh->init(&header.params);
  322. if (dtStatusFailed(status))
  323. {
  324. fclose(f);
  325. return;
  326. }
  327. // Read tiles.
  328. for (int i = 0; i < header.numTiles; ++i)
  329. {
  330. NavMeshTileHeader tileHeader;
  331. readLen = fread(&tileHeader, sizeof(tileHeader), 1, f);
  332. if (readLen != 1)
  333. {
  334. fclose(f);
  335. return;
  336. }
  337. if (!tileHeader.tileRef || !tileHeader.dataSize)
  338. break;
  339. unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
  340. if (!data) break;
  341. memset(data, 0, tileHeader.dataSize);
  342. readLen = fread(data, tileHeader.dataSize, 1, f);
  343. if (readLen != 1)
  344. {
  345. dtFree(data);
  346. fclose(f);
  347. return;
  348. }
  349. mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
  350. }
  351. m_impl->nav_mesh = mesh;
  352. LogWrite(MAP__INFO, 7, "Map", "Loaded Navmesh File [{%s}]", path.c_str());
  353. }
  354. }
  355. void PathfinderNavmesh::ShowPath(Client * c, const glm::vec3 &start, const glm::vec3 &end)
  356. {
  357. /* auto &list = entity_list.GetNPCList();
  358. for (auto &iter : list) {
  359. auto npc = iter.second;
  360. auto name = npc->GetName();
  361. if (strstr(name, "PathNode") != nullptr) {
  362. npc->Depop();
  363. }
  364. }
  365. PathfinderOptions opts;
  366. opts.smooth_path = true;
  367. opts.step_size = RuleR(Pathing, NavmeshStepSize);
  368. bool partial = false;
  369. bool stuck = false;
  370. auto path = FindPath(start, end, partial, stuck, opts);
  371. for (auto &node : path) {
  372. if (!node.teleport) {
  373. NPC::SpawnNPC("PathNode 2253 1 0 1 2 1", glm::vec4(node.pos, 1.0));
  374. }
  375. }*/
  376. }
  377. dtStatus PathfinderNavmesh::GetPolyHeightNoConnections(dtPolyRef ref, const float *pos, float *height) const
  378. {
  379. auto *m_nav = m_impl->nav_mesh;
  380. if (!m_nav) {
  381. return DT_FAILURE;
  382. }
  383. const dtMeshTile* tile = 0;
  384. const dtPoly* poly = 0;
  385. if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) {
  386. return DT_FAILURE | DT_INVALID_PARAM;
  387. }
  388. if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) {
  389. const unsigned int ip = (unsigned int)(poly - tile->polys);
  390. const dtPolyDetail* pd = &tile->detailMeshes[ip];
  391. for (int j = 0; j < pd->triCount; ++j)
  392. {
  393. const unsigned char* t = &tile->detailTris[(pd->triBase + j) * 4];
  394. const float* v[3];
  395. for (int k = 0; k < 3; ++k)
  396. {
  397. if (t[k] < poly->vertCount)
  398. v[k] = &tile->verts[poly->verts[t[k]] * 3];
  399. else
  400. v[k] = &tile->detailVerts[(pd->vertBase + (t[k] - poly->vertCount)) * 3];
  401. }
  402. float h;
  403. if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
  404. {
  405. if (height)
  406. *height = h;
  407. return DT_SUCCESS;
  408. }
  409. }
  410. }
  411. return DT_FAILURE | DT_INVALID_PARAM;
  412. }
  413. dtStatus PathfinderNavmesh::GetPolyHeightOnPath(const dtPolyRef *path, const int path_len, const glm::vec3 &pos, float *h) const
  414. {
  415. if (!path || !path_len) {
  416. return DT_FAILURE;
  417. }
  418. for (int i = 0; i < path_len; ++i) {
  419. dtPolyRef ref = path[i];
  420. if (dtStatusSucceed(GetPolyHeightNoConnections(ref, &pos[0], h))) {
  421. return DT_SUCCESS;
  422. }
  423. }
  424. return DT_FAILURE;
  425. }