nav_map.cpp 34 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975
  1. /**************************************************************************/
  2. /* nav_map.cpp */
  3. /**************************************************************************/
  4. /* This file is part of: */
  5. /* GODOT ENGINE */
  6. /* https://godotengine.org */
  7. /**************************************************************************/
  8. /* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
  9. /* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
  10. /* */
  11. /* Permission is hereby granted, free of charge, to any person obtaining */
  12. /* a copy of this software and associated documentation files (the */
  13. /* "Software"), to deal in the Software without restriction, including */
  14. /* without limitation the rights to use, copy, modify, merge, publish, */
  15. /* distribute, sublicense, and/or sell copies of the Software, and to */
  16. /* permit persons to whom the Software is furnished to do so, subject to */
  17. /* the following conditions: */
  18. /* */
  19. /* The above copyright notice and this permission notice shall be */
  20. /* included in all copies or substantial portions of the Software. */
  21. /* */
  22. /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
  23. /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
  24. /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
  25. /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
  26. /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
  27. /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
  28. /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
  29. /**************************************************************************/
  30. #include "nav_map.h"
  31. #include "nav_agent.h"
  32. #include "nav_link.h"
  33. #include "nav_obstacle.h"
  34. #include "nav_region.h"
  35. #include "3d/nav_mesh_queries_3d.h"
  36. #include "core/config/project_settings.h"
  37. #include "core/object/worker_thread_pool.h"
  38. #include <Obstacle2d.h>
  39. #ifdef DEBUG_ENABLED
  40. #define NAVMAP_ITERATION_ZERO_ERROR_MSG() \
  41. ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\
  42. NavigationServer 'map_changed' signal can be used to receive update notifications.\n\
  43. NavigationServer 'map_get_iteration_id()' can be used to check if a map has finished its newest iteration.");
  44. #else
  45. #define NAVMAP_ITERATION_ZERO_ERROR_MSG()
  46. #endif // DEBUG_ENABLED
  47. void NavMap::set_up(Vector3 p_up) {
  48. if (up == p_up) {
  49. return;
  50. }
  51. up = p_up;
  52. map_settings_dirty = true;
  53. }
  54. void NavMap::set_cell_size(real_t p_cell_size) {
  55. if (cell_size == p_cell_size) {
  56. return;
  57. }
  58. cell_size = p_cell_size;
  59. _update_merge_rasterizer_cell_dimensions();
  60. map_settings_dirty = true;
  61. }
  62. void NavMap::set_cell_height(real_t p_cell_height) {
  63. if (cell_height == p_cell_height) {
  64. return;
  65. }
  66. cell_height = p_cell_height;
  67. _update_merge_rasterizer_cell_dimensions();
  68. map_settings_dirty = true;
  69. }
  70. void NavMap::set_merge_rasterizer_cell_scale(float p_value) {
  71. if (merge_rasterizer_cell_scale == p_value) {
  72. return;
  73. }
  74. merge_rasterizer_cell_scale = p_value;
  75. _update_merge_rasterizer_cell_dimensions();
  76. map_settings_dirty = true;
  77. }
  78. void NavMap::set_use_edge_connections(bool p_enabled) {
  79. if (use_edge_connections == p_enabled) {
  80. return;
  81. }
  82. use_edge_connections = p_enabled;
  83. iteration_dirty = true;
  84. }
  85. void NavMap::set_edge_connection_margin(real_t p_edge_connection_margin) {
  86. if (edge_connection_margin == p_edge_connection_margin) {
  87. return;
  88. }
  89. edge_connection_margin = p_edge_connection_margin;
  90. iteration_dirty = true;
  91. }
  92. void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
  93. if (link_connection_radius == p_link_connection_radius) {
  94. return;
  95. }
  96. link_connection_radius = p_link_connection_radius;
  97. iteration_dirty = true;
  98. }
  99. gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
  100. const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size));
  101. const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_height));
  102. const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size));
  103. gd::PointKey p;
  104. p.key = 0;
  105. p.x = x;
  106. p.y = y;
  107. p.z = z;
  108. return p;
  109. }
  110. Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
  111. RWLockRead read_lock(map_rwlock);
  112. if (iteration_id == 0) {
  113. NAVMAP_ITERATION_ZERO_ERROR_MSG();
  114. return Vector<Vector3>();
  115. }
  116. return NavMeshQueries3D::polygons_get_path(
  117. polygons, p_origin, p_destination, p_optimize, p_navigation_layers,
  118. r_path_types, r_path_rids, r_path_owners, up, link_polygons.size());
  119. }
  120. Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
  121. RWLockRead read_lock(map_rwlock);
  122. if (iteration_id == 0) {
  123. NAVMAP_ITERATION_ZERO_ERROR_MSG();
  124. return Vector3();
  125. }
  126. return NavMeshQueries3D::polygons_get_closest_point_to_segment(polygons, p_from, p_to, p_use_collision);
  127. }
  128. Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
  129. RWLockRead read_lock(map_rwlock);
  130. if (iteration_id == 0) {
  131. NAVMAP_ITERATION_ZERO_ERROR_MSG();
  132. return Vector3();
  133. }
  134. return NavMeshQueries3D::polygons_get_closest_point(polygons, p_point);
  135. }
  136. Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
  137. RWLockRead read_lock(map_rwlock);
  138. if (iteration_id == 0) {
  139. NAVMAP_ITERATION_ZERO_ERROR_MSG();
  140. return Vector3();
  141. }
  142. return NavMeshQueries3D::polygons_get_closest_point_normal(polygons, p_point);
  143. }
  144. RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
  145. RWLockRead read_lock(map_rwlock);
  146. if (iteration_id == 0) {
  147. NAVMAP_ITERATION_ZERO_ERROR_MSG();
  148. return RID();
  149. }
  150. return NavMeshQueries3D::polygons_get_closest_point_owner(polygons, p_point);
  151. }
  152. gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
  153. RWLockRead read_lock(map_rwlock);
  154. return NavMeshQueries3D::polygons_get_closest_point_info(polygons, p_point);
  155. }
  156. void NavMap::add_region(NavRegion *p_region) {
  157. regions.push_back(p_region);
  158. iteration_dirty = true;
  159. }
  160. void NavMap::remove_region(NavRegion *p_region) {
  161. int64_t region_index = regions.find(p_region);
  162. if (region_index >= 0) {
  163. regions.remove_at_unordered(region_index);
  164. iteration_dirty = true;
  165. }
  166. }
  167. void NavMap::add_link(NavLink *p_link) {
  168. links.push_back(p_link);
  169. iteration_dirty = true;
  170. }
  171. void NavMap::remove_link(NavLink *p_link) {
  172. int64_t link_index = links.find(p_link);
  173. if (link_index >= 0) {
  174. links.remove_at_unordered(link_index);
  175. iteration_dirty = true;
  176. }
  177. }
  178. bool NavMap::has_agent(NavAgent *agent) const {
  179. return agents.has(agent);
  180. }
  181. void NavMap::add_agent(NavAgent *agent) {
  182. if (!has_agent(agent)) {
  183. agents.push_back(agent);
  184. agents_dirty = true;
  185. }
  186. }
  187. void NavMap::remove_agent(NavAgent *agent) {
  188. remove_agent_as_controlled(agent);
  189. int64_t agent_index = agents.find(agent);
  190. if (agent_index >= 0) {
  191. agents.remove_at_unordered(agent_index);
  192. agents_dirty = true;
  193. }
  194. }
  195. bool NavMap::has_obstacle(NavObstacle *obstacle) const {
  196. return obstacles.has(obstacle);
  197. }
  198. void NavMap::add_obstacle(NavObstacle *obstacle) {
  199. if (obstacle->get_paused()) {
  200. // No point in adding a paused obstacle, it will add itself when unpaused again.
  201. return;
  202. }
  203. if (!has_obstacle(obstacle)) {
  204. obstacles.push_back(obstacle);
  205. obstacles_dirty = true;
  206. }
  207. }
  208. void NavMap::remove_obstacle(NavObstacle *obstacle) {
  209. int64_t obstacle_index = obstacles.find(obstacle);
  210. if (obstacle_index >= 0) {
  211. obstacles.remove_at_unordered(obstacle_index);
  212. obstacles_dirty = true;
  213. }
  214. }
  215. void NavMap::set_agent_as_controlled(NavAgent *agent) {
  216. remove_agent_as_controlled(agent);
  217. if (agent->get_paused()) {
  218. // No point in adding a paused agent, it will add itself when unpaused again.
  219. return;
  220. }
  221. if (agent->get_use_3d_avoidance()) {
  222. int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
  223. if (agent_3d_index < 0) {
  224. active_3d_avoidance_agents.push_back(agent);
  225. agents_dirty = true;
  226. }
  227. } else {
  228. int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
  229. if (agent_2d_index < 0) {
  230. active_2d_avoidance_agents.push_back(agent);
  231. agents_dirty = true;
  232. }
  233. }
  234. }
  235. void NavMap::remove_agent_as_controlled(NavAgent *agent) {
  236. int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
  237. if (agent_3d_index >= 0) {
  238. active_3d_avoidance_agents.remove_at_unordered(agent_3d_index);
  239. agents_dirty = true;
  240. }
  241. int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
  242. if (agent_2d_index >= 0) {
  243. active_2d_avoidance_agents.remove_at_unordered(agent_2d_index);
  244. agents_dirty = true;
  245. }
  246. }
  247. Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
  248. RWLockRead read_lock(map_rwlock);
  249. const LocalVector<NavRegion *> map_regions = get_regions();
  250. if (map_regions.is_empty()) {
  251. return Vector3();
  252. }
  253. LocalVector<const NavRegion *> accessible_regions;
  254. for (const NavRegion *region : map_regions) {
  255. if (!region->get_enabled() || (p_navigation_layers & region->get_navigation_layers()) == 0) {
  256. continue;
  257. }
  258. accessible_regions.push_back(region);
  259. }
  260. if (accessible_regions.is_empty()) {
  261. // All existing region polygons are disabled.
  262. return Vector3();
  263. }
  264. if (p_uniformly) {
  265. real_t accumulated_region_surface_area = 0;
  266. RBMap<real_t, uint32_t> accessible_regions_area_map;
  267. for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) {
  268. const NavRegion *region = accessible_regions[accessible_region_index];
  269. real_t region_surface_area = region->get_surface_area();
  270. if (region_surface_area == 0.0f) {
  271. continue;
  272. }
  273. accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index;
  274. accumulated_region_surface_area += region_surface_area;
  275. }
  276. if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) {
  277. // All faces have no real surface / no area.
  278. return Vector3();
  279. }
  280. real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area);
  281. RBMap<real_t, uint32_t>::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map);
  282. ERR_FAIL_COND_V(!E, Vector3());
  283. uint32_t random_region_index = E->value;
  284. ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
  285. const NavRegion *random_region = accessible_regions[random_region_index];
  286. ERR_FAIL_NULL_V(random_region, Vector3());
  287. return random_region->get_random_point(p_navigation_layers, p_uniformly);
  288. } else {
  289. uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
  290. const NavRegion *random_region = accessible_regions[random_region_index];
  291. ERR_FAIL_NULL_V(random_region, Vector3());
  292. return random_region->get_random_point(p_navigation_layers, p_uniformly);
  293. }
  294. }
  295. void NavMap::sync() {
  296. RWLockWrite write_lock(map_rwlock);
  297. performance_data.pm_region_count = regions.size();
  298. performance_data.pm_agent_count = agents.size();
  299. performance_data.pm_link_count = links.size();
  300. performance_data.pm_obstacle_count = obstacles.size();
  301. _sync_dirty_map_update_requests();
  302. if (iteration_dirty) {
  303. performance_data.pm_polygon_count = 0;
  304. performance_data.pm_edge_count = 0;
  305. performance_data.pm_edge_merge_count = 0;
  306. performance_data.pm_edge_connection_count = 0;
  307. performance_data.pm_edge_free_count = 0;
  308. // Remove regions connections.
  309. region_external_connections.clear();
  310. for (NavRegion *region : regions) {
  311. region_external_connections[region] = LocalVector<gd::Edge::Connection>();
  312. }
  313. // Resize the polygon count.
  314. int polygon_count = 0;
  315. for (const NavRegion *region : regions) {
  316. if (!region->get_enabled()) {
  317. continue;
  318. }
  319. polygon_count += region->get_polygons().size();
  320. }
  321. polygons.resize(polygon_count);
  322. // Copy all region polygons in the map.
  323. polygon_count = 0;
  324. for (const NavRegion *region : regions) {
  325. if (!region->get_enabled()) {
  326. continue;
  327. }
  328. const LocalVector<gd::Polygon> &polygons_source = region->get_polygons();
  329. for (uint32_t n = 0; n < polygons_source.size(); n++) {
  330. polygons[polygon_count] = polygons_source[n];
  331. polygons[polygon_count].id = polygon_count;
  332. polygon_count++;
  333. }
  334. }
  335. performance_data.pm_polygon_count = polygon_count;
  336. // Group all edges per key.
  337. connection_pairs_map.clear();
  338. connection_pairs_map.reserve(polygons.size());
  339. int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
  340. for (gd::Polygon &poly : polygons) {
  341. for (uint32_t p = 0; p < poly.points.size(); p++) {
  342. const int next_point = (p + 1) % poly.points.size();
  343. const gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
  344. HashMap<gd::EdgeKey, ConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
  345. if (!pair_it) {
  346. pair_it = connection_pairs_map.insert(ek, ConnectionPair());
  347. performance_data.pm_edge_count += 1;
  348. ++free_edges_count;
  349. }
  350. ConnectionPair &pair = pair_it->value;
  351. if (pair.size < 2) {
  352. // Add the polygon/edge tuple to this key.
  353. gd::Edge::Connection new_connection;
  354. new_connection.polygon = &poly;
  355. new_connection.edge = p;
  356. new_connection.pathway_start = poly.points[p].pos;
  357. new_connection.pathway_end = poly.points[next_point].pos;
  358. pair.connections[pair.size] = new_connection;
  359. ++pair.size;
  360. if (pair.size == 2) {
  361. --free_edges_count;
  362. }
  363. } else {
  364. // The edge is already connected with another edge, skip.
  365. ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
  366. }
  367. }
  368. }
  369. free_edges.clear();
  370. free_edges.reserve(free_edges_count);
  371. for (const KeyValue<gd::EdgeKey, ConnectionPair> &pair_it : connection_pairs_map) {
  372. const ConnectionPair &pair = pair_it.value;
  373. if (pair.size == 2) {
  374. // Connect edge that are shared in different polygons.
  375. const gd::Edge::Connection &c1 = pair.connections[0];
  376. const gd::Edge::Connection &c2 = pair.connections[1];
  377. c1.polygon->edges[c1.edge].connections.push_back(c2);
  378. c2.polygon->edges[c2.edge].connections.push_back(c1);
  379. // Note: The pathway_start/end are full for those connection and do not need to be modified.
  380. performance_data.pm_edge_merge_count += 1;
  381. } else {
  382. CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
  383. if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
  384. free_edges.push_back(pair.connections[0]);
  385. }
  386. }
  387. }
  388. // Find the compatible near edges.
  389. //
  390. // Note:
  391. // Considering that the edges must be compatible (for obvious reasons)
  392. // to be connected, create new polygons to remove that small gap is
  393. // not really useful and would result in wasteful computation during
  394. // connection, integration and path finding.
  395. performance_data.pm_edge_free_count = free_edges.size();
  396. const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
  397. for (uint32_t i = 0; i < free_edges.size(); i++) {
  398. const gd::Edge::Connection &free_edge = free_edges[i];
  399. Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
  400. Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
  401. for (uint32_t j = 0; j < free_edges.size(); j++) {
  402. const gd::Edge::Connection &other_edge = free_edges[j];
  403. if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
  404. continue;
  405. }
  406. Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
  407. Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
  408. // Compute the projection of the opposite edge on the current one
  409. Vector3 edge_vector = edge_p2 - edge_p1;
  410. real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
  411. real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
  412. if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
  413. continue;
  414. }
  415. // Check if the two edges are close to each other enough and compute a pathway between the two regions.
  416. Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
  417. Vector3 other1;
  418. if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
  419. other1 = other_edge_p1;
  420. } else {
  421. other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
  422. }
  423. if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
  424. continue;
  425. }
  426. Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
  427. Vector3 other2;
  428. if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
  429. other2 = other_edge_p2;
  430. } else {
  431. other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
  432. }
  433. if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
  434. continue;
  435. }
  436. // The edges can now be connected.
  437. gd::Edge::Connection new_connection = other_edge;
  438. new_connection.pathway_start = (self1 + other1) / 2.0;
  439. new_connection.pathway_end = (self2 + other2) / 2.0;
  440. free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
  441. // Add the connection to the region_connection map.
  442. region_external_connections[(NavRegion *)free_edge.polygon->owner].push_back(new_connection);
  443. performance_data.pm_edge_connection_count += 1;
  444. }
  445. }
  446. uint32_t link_poly_idx = 0;
  447. link_polygons.resize(links.size());
  448. // Search for polygons within range of a nav link.
  449. for (const NavLink *link : links) {
  450. if (!link->get_enabled()) {
  451. continue;
  452. }
  453. const Vector3 start = link->get_start_position();
  454. const Vector3 end = link->get_end_position();
  455. gd::Polygon *closest_start_polygon = nullptr;
  456. real_t closest_start_sqr_dist = link_connection_radius * link_connection_radius;
  457. Vector3 closest_start_point;
  458. gd::Polygon *closest_end_polygon = nullptr;
  459. real_t closest_end_sqr_dist = link_connection_radius * link_connection_radius;
  460. Vector3 closest_end_point;
  461. // Create link to any polygons within the search radius of the start point.
  462. for (uint32_t start_index = 0; start_index < polygons.size(); start_index++) {
  463. gd::Polygon &start_poly = polygons[start_index];
  464. // For each face check the distance to the start
  465. for (uint32_t start_point_id = 2; start_point_id < start_poly.points.size(); start_point_id += 1) {
  466. const Face3 start_face(start_poly.points[0].pos, start_poly.points[start_point_id - 1].pos, start_poly.points[start_point_id].pos);
  467. const Vector3 start_point = start_face.get_closest_point_to(start);
  468. const real_t sqr_dist = start_point.distance_squared_to(start);
  469. // Pick the polygon that is within our radius and is closer than anything we've seen yet.
  470. if (sqr_dist < closest_start_sqr_dist) {
  471. closest_start_sqr_dist = sqr_dist;
  472. closest_start_point = start_point;
  473. closest_start_polygon = &start_poly;
  474. }
  475. }
  476. }
  477. // Find any polygons within the search radius of the end point.
  478. for (gd::Polygon &end_poly : polygons) {
  479. // For each face check the distance to the end
  480. for (uint32_t end_point_id = 2; end_point_id < end_poly.points.size(); end_point_id += 1) {
  481. const Face3 end_face(end_poly.points[0].pos, end_poly.points[end_point_id - 1].pos, end_poly.points[end_point_id].pos);
  482. const Vector3 end_point = end_face.get_closest_point_to(end);
  483. const real_t sqr_dist = end_point.distance_squared_to(end);
  484. // Pick the polygon that is within our radius and is closer than anything we've seen yet.
  485. if (sqr_dist < closest_end_sqr_dist) {
  486. closest_end_sqr_dist = sqr_dist;
  487. closest_end_point = end_point;
  488. closest_end_polygon = &end_poly;
  489. }
  490. }
  491. }
  492. // If we have both a start and end point, then create a synthetic polygon to route through.
  493. if (closest_start_polygon && closest_end_polygon) {
  494. gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
  495. new_polygon.id = polygon_count++;
  496. new_polygon.owner = link;
  497. new_polygon.edges.clear();
  498. new_polygon.edges.resize(4);
  499. new_polygon.points.clear();
  500. new_polygon.points.reserve(4);
  501. // Build a set of vertices that create a thin polygon going from the start to the end point.
  502. new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
  503. new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
  504. new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
  505. new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
  506. // Setup connections to go forward in the link.
  507. {
  508. gd::Edge::Connection entry_connection;
  509. entry_connection.polygon = &new_polygon;
  510. entry_connection.edge = -1;
  511. entry_connection.pathway_start = new_polygon.points[0].pos;
  512. entry_connection.pathway_end = new_polygon.points[1].pos;
  513. closest_start_polygon->edges[0].connections.push_back(entry_connection);
  514. gd::Edge::Connection exit_connection;
  515. exit_connection.polygon = closest_end_polygon;
  516. exit_connection.edge = -1;
  517. exit_connection.pathway_start = new_polygon.points[2].pos;
  518. exit_connection.pathway_end = new_polygon.points[3].pos;
  519. new_polygon.edges[2].connections.push_back(exit_connection);
  520. }
  521. // If the link is bi-directional, create connections from the end to the start.
  522. if (link->is_bidirectional()) {
  523. gd::Edge::Connection entry_connection;
  524. entry_connection.polygon = &new_polygon;
  525. entry_connection.edge = -1;
  526. entry_connection.pathway_start = new_polygon.points[2].pos;
  527. entry_connection.pathway_end = new_polygon.points[3].pos;
  528. closest_end_polygon->edges[0].connections.push_back(entry_connection);
  529. gd::Edge::Connection exit_connection;
  530. exit_connection.polygon = closest_start_polygon;
  531. exit_connection.edge = -1;
  532. exit_connection.pathway_start = new_polygon.points[0].pos;
  533. exit_connection.pathway_end = new_polygon.points[1].pos;
  534. new_polygon.edges[0].connections.push_back(exit_connection);
  535. }
  536. }
  537. }
  538. // Some code treats 0 as a failure case, so we avoid returning 0 and modulo wrap UINT32_MAX manually.
  539. iteration_id = iteration_id % UINT32_MAX + 1;
  540. }
  541. map_settings_dirty = false;
  542. iteration_dirty = false;
  543. _sync_avoidance();
  544. }
  545. void NavMap::_sync_avoidance() {
  546. _sync_dirty_avoidance_update_requests();
  547. if (obstacles_dirty || agents_dirty) {
  548. _update_rvo_simulation();
  549. }
  550. obstacles_dirty = false;
  551. agents_dirty = false;
  552. }
  553. void NavMap::_update_rvo_obstacles_tree_2d() {
  554. int obstacle_vertex_count = 0;
  555. for (NavObstacle *obstacle : obstacles) {
  556. obstacle_vertex_count += obstacle->get_vertices().size();
  557. }
  558. // Cleaning old obstacles.
  559. for (size_t i = 0; i < rvo_simulation_2d.obstacles_.size(); ++i) {
  560. delete rvo_simulation_2d.obstacles_[i];
  561. }
  562. rvo_simulation_2d.obstacles_.clear();
  563. // Cannot use LocalVector here as RVO library expects std::vector to build KdTree
  564. std::vector<RVO2D::Obstacle2D *> &raw_obstacles = rvo_simulation_2d.obstacles_;
  565. raw_obstacles.reserve(obstacle_vertex_count);
  566. // The following block is modified copy from RVO2D::AddObstacle()
  567. // Obstacles are linked and depend on all other obstacles.
  568. for (NavObstacle *obstacle : obstacles) {
  569. const Vector3 &_obstacle_position = obstacle->get_position();
  570. const Vector<Vector3> &_obstacle_vertices = obstacle->get_vertices();
  571. if (_obstacle_vertices.size() < 2) {
  572. continue;
  573. }
  574. std::vector<RVO2D::Vector2> rvo_2d_vertices;
  575. rvo_2d_vertices.reserve(_obstacle_vertices.size());
  576. uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
  577. real_t _obstacle_height = obstacle->get_height();
  578. for (const Vector3 &_obstacle_vertex : _obstacle_vertices) {
  579. #ifdef TOOLS_ENABLED
  580. if (_obstacle_vertex.y != 0) {
  581. WARN_PRINT_ONCE("Y coordinates of static obstacle vertices are ignored. Please use obstacle position Y to change elevation of obstacle.");
  582. }
  583. #endif
  584. rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z));
  585. }
  586. const size_t obstacleNo = raw_obstacles.size();
  587. for (size_t i = 0; i < rvo_2d_vertices.size(); i++) {
  588. RVO2D::Obstacle2D *rvo_2d_obstacle = new RVO2D::Obstacle2D();
  589. rvo_2d_obstacle->point_ = rvo_2d_vertices[i];
  590. rvo_2d_obstacle->height_ = _obstacle_height;
  591. rvo_2d_obstacle->elevation_ = _obstacle_position.y;
  592. rvo_2d_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
  593. if (i != 0) {
  594. rvo_2d_obstacle->prevObstacle_ = raw_obstacles.back();
  595. rvo_2d_obstacle->prevObstacle_->nextObstacle_ = rvo_2d_obstacle;
  596. }
  597. if (i == rvo_2d_vertices.size() - 1) {
  598. rvo_2d_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
  599. rvo_2d_obstacle->nextObstacle_->prevObstacle_ = rvo_2d_obstacle;
  600. }
  601. rvo_2d_obstacle->unitDir_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]);
  602. if (rvo_2d_vertices.size() == 2) {
  603. rvo_2d_obstacle->isConvex_ = true;
  604. } else {
  605. rvo_2d_obstacle->isConvex_ = (leftOf(rvo_2d_vertices[(i == 0 ? rvo_2d_vertices.size() - 1 : i - 1)], rvo_2d_vertices[i], rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
  606. }
  607. rvo_2d_obstacle->id_ = raw_obstacles.size();
  608. raw_obstacles.push_back(rvo_2d_obstacle);
  609. }
  610. }
  611. rvo_simulation_2d.kdTree_->buildObstacleTree(raw_obstacles);
  612. }
  613. void NavMap::_update_rvo_agents_tree_2d() {
  614. // Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
  615. std::vector<RVO2D::Agent2D *> raw_agents;
  616. raw_agents.reserve(active_2d_avoidance_agents.size());
  617. for (NavAgent *agent : active_2d_avoidance_agents) {
  618. raw_agents.push_back(agent->get_rvo_agent_2d());
  619. }
  620. rvo_simulation_2d.kdTree_->buildAgentTree(raw_agents);
  621. }
  622. void NavMap::_update_rvo_agents_tree_3d() {
  623. // Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
  624. std::vector<RVO3D::Agent3D *> raw_agents;
  625. raw_agents.reserve(active_3d_avoidance_agents.size());
  626. for (NavAgent *agent : active_3d_avoidance_agents) {
  627. raw_agents.push_back(agent->get_rvo_agent_3d());
  628. }
  629. rvo_simulation_3d.kdTree_->buildAgentTree(raw_agents);
  630. }
  631. void NavMap::_update_rvo_simulation() {
  632. if (obstacles_dirty) {
  633. _update_rvo_obstacles_tree_2d();
  634. }
  635. if (agents_dirty) {
  636. _update_rvo_agents_tree_2d();
  637. _update_rvo_agents_tree_3d();
  638. }
  639. }
  640. void NavMap::compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent) {
  641. (*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
  642. (*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
  643. (*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d);
  644. (*(agent + index))->update();
  645. }
  646. void NavMap::compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent) {
  647. (*(agent + index))->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
  648. (*(agent + index))->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
  649. (*(agent + index))->get_rvo_agent_3d()->update(&rvo_simulation_3d);
  650. (*(agent + index))->update();
  651. }
  652. void NavMap::step(real_t p_deltatime) {
  653. deltatime = p_deltatime;
  654. rvo_simulation_2d.setTimeStep(float(deltatime));
  655. rvo_simulation_3d.setTimeStep(float(deltatime));
  656. if (active_2d_avoidance_agents.size() > 0) {
  657. if (use_threads && avoidance_use_multiple_threads) {
  658. WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
  659. WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
  660. } else {
  661. for (NavAgent *agent : active_2d_avoidance_agents) {
  662. agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
  663. agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
  664. agent->get_rvo_agent_2d()->update(&rvo_simulation_2d);
  665. agent->update();
  666. }
  667. }
  668. }
  669. if (active_3d_avoidance_agents.size() > 0) {
  670. if (use_threads && avoidance_use_multiple_threads) {
  671. WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D"));
  672. WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
  673. } else {
  674. for (NavAgent *agent : active_3d_avoidance_agents) {
  675. agent->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
  676. agent->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
  677. agent->get_rvo_agent_3d()->update(&rvo_simulation_3d);
  678. agent->update();
  679. }
  680. }
  681. }
  682. }
  683. void NavMap::dispatch_callbacks() {
  684. for (NavAgent *agent : active_2d_avoidance_agents) {
  685. agent->dispatch_avoidance_callback();
  686. }
  687. for (NavAgent *agent : active_3d_avoidance_agents) {
  688. agent->dispatch_avoidance_callback();
  689. }
  690. }
  691. void NavMap::_update_merge_rasterizer_cell_dimensions() {
  692. merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale;
  693. merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale;
  694. }
  695. int NavMap::get_region_connections_count(NavRegion *p_region) const {
  696. ERR_FAIL_NULL_V(p_region, 0);
  697. HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region);
  698. if (found_connections) {
  699. return found_connections->value.size();
  700. }
  701. return 0;
  702. }
  703. Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const {
  704. ERR_FAIL_NULL_V(p_region, Vector3());
  705. HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region);
  706. if (found_connections) {
  707. ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
  708. return found_connections->value[p_connection_id].pathway_start;
  709. }
  710. return Vector3();
  711. }
  712. Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const {
  713. ERR_FAIL_NULL_V(p_region, Vector3());
  714. HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region);
  715. if (found_connections) {
  716. ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
  717. return found_connections->value[p_connection_id].pathway_end;
  718. }
  719. return Vector3();
  720. }
  721. void NavMap::add_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request) {
  722. if (p_sync_request->in_list()) {
  723. return;
  724. }
  725. sync_dirty_requests.regions.add(p_sync_request);
  726. }
  727. void NavMap::add_link_sync_dirty_request(SelfList<NavLink> *p_sync_request) {
  728. if (p_sync_request->in_list()) {
  729. return;
  730. }
  731. sync_dirty_requests.links.add(p_sync_request);
  732. }
  733. void NavMap::add_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request) {
  734. if (p_sync_request->in_list()) {
  735. return;
  736. }
  737. sync_dirty_requests.agents.add(p_sync_request);
  738. }
  739. void NavMap::add_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request) {
  740. if (p_sync_request->in_list()) {
  741. return;
  742. }
  743. sync_dirty_requests.obstacles.add(p_sync_request);
  744. }
  745. void NavMap::remove_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request) {
  746. if (!p_sync_request->in_list()) {
  747. return;
  748. }
  749. sync_dirty_requests.regions.remove(p_sync_request);
  750. }
  751. void NavMap::remove_link_sync_dirty_request(SelfList<NavLink> *p_sync_request) {
  752. if (!p_sync_request->in_list()) {
  753. return;
  754. }
  755. sync_dirty_requests.links.remove(p_sync_request);
  756. }
  757. void NavMap::remove_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request) {
  758. if (!p_sync_request->in_list()) {
  759. return;
  760. }
  761. sync_dirty_requests.agents.remove(p_sync_request);
  762. }
  763. void NavMap::remove_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request) {
  764. if (!p_sync_request->in_list()) {
  765. return;
  766. }
  767. sync_dirty_requests.obstacles.remove(p_sync_request);
  768. }
  769. void NavMap::_sync_dirty_map_update_requests() {
  770. // If entire map settings changed make all regions dirty.
  771. if (map_settings_dirty) {
  772. for (NavRegion *region : regions) {
  773. region->scratch_polygons();
  774. }
  775. iteration_dirty = true;
  776. }
  777. if (!iteration_dirty) {
  778. iteration_dirty = sync_dirty_requests.regions.first() || sync_dirty_requests.links.first();
  779. }
  780. // Sync NavRegions.
  781. for (SelfList<NavRegion> *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
  782. element->self()->sync();
  783. }
  784. sync_dirty_requests.regions.clear();
  785. // Sync NavLinks.
  786. for (SelfList<NavLink> *element = sync_dirty_requests.links.first(); element; element = element->next()) {
  787. element->self()->sync();
  788. }
  789. sync_dirty_requests.links.clear();
  790. }
  791. void NavMap::_sync_dirty_avoidance_update_requests() {
  792. // Sync NavAgents.
  793. if (!agents_dirty) {
  794. agents_dirty = sync_dirty_requests.agents.first();
  795. }
  796. for (SelfList<NavAgent> *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
  797. element->self()->sync();
  798. }
  799. sync_dirty_requests.agents.clear();
  800. // Sync NavObstacles.
  801. if (!obstacles_dirty) {
  802. obstacles_dirty = sync_dirty_requests.obstacles.first();
  803. }
  804. for (SelfList<NavObstacle> *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
  805. element->self()->sync();
  806. }
  807. sync_dirty_requests.obstacles.clear();
  808. }
  809. NavMap::NavMap() {
  810. avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
  811. avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
  812. }
  813. NavMap::~NavMap() {
  814. }