nav_region_3d.cpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339
  1. /**************************************************************************/
  2. /* nav_region_3d.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_region_3d.h"
  31. #include "nav_map_3d.h"
  32. #include "3d/nav_map_builder_3d.h"
  33. #include "3d/nav_mesh_queries_3d.h"
  34. #include "3d/nav_region_iteration_3d.h"
  35. using namespace Nav3D;
  36. void NavRegion3D::set_map(NavMap3D *p_map) {
  37. if (map == p_map) {
  38. return;
  39. }
  40. cancel_sync_request();
  41. if (map) {
  42. map->remove_region(this);
  43. }
  44. map = p_map;
  45. polygons_dirty = true;
  46. if (map) {
  47. map->add_region(this);
  48. request_sync();
  49. }
  50. }
  51. void NavRegion3D::set_enabled(bool p_enabled) {
  52. if (enabled == p_enabled) {
  53. return;
  54. }
  55. enabled = p_enabled;
  56. // TODO: This should not require a full rebuild as the region has not really changed.
  57. polygons_dirty = true;
  58. request_sync();
  59. }
  60. void NavRegion3D::set_use_edge_connections(bool p_enabled) {
  61. if (use_edge_connections != p_enabled) {
  62. use_edge_connections = p_enabled;
  63. polygons_dirty = true;
  64. }
  65. request_sync();
  66. }
  67. void NavRegion3D::set_transform(Transform3D p_transform) {
  68. if (transform == p_transform) {
  69. return;
  70. }
  71. transform = p_transform;
  72. polygons_dirty = true;
  73. request_sync();
  74. #ifdef DEBUG_ENABLED
  75. if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) {
  76. ERR_PRINT_ONCE("Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation.");
  77. }
  78. #endif // DEBUG_ENABLED
  79. }
  80. void NavRegion3D::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
  81. #ifdef DEBUG_ENABLED
  82. if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->get_cell_size()))) {
  83. ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_size()), double(map->get_cell_size())));
  84. }
  85. if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_height()), double(p_navigation_mesh->get_cell_height()))) {
  86. ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_height()), double(map->get_cell_height())));
  87. }
  88. #endif // DEBUG_ENABLED
  89. RWLockWrite write_lock(navmesh_rwlock);
  90. pending_navmesh_vertices.clear();
  91. pending_navmesh_polygons.clear();
  92. if (p_navigation_mesh.is_valid()) {
  93. p_navigation_mesh->get_data(pending_navmesh_vertices, pending_navmesh_polygons);
  94. }
  95. polygons_dirty = true;
  96. request_sync();
  97. }
  98. Vector3 NavRegion3D::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const {
  99. RWLockRead read_lock(region_rwlock);
  100. return NavMeshQueries3D::polygons_get_closest_point_to_segment(
  101. get_polygons(), p_from, p_to, p_use_collision);
  102. }
  103. ClosestPointQueryResult NavRegion3D::get_closest_point_info(const Vector3 &p_point) const {
  104. RWLockRead read_lock(region_rwlock);
  105. return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point);
  106. }
  107. Vector3 NavRegion3D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
  108. RWLockRead read_lock(region_rwlock);
  109. if (!get_enabled()) {
  110. return Vector3();
  111. }
  112. return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
  113. }
  114. void NavRegion3D::set_navigation_layers(uint32_t p_navigation_layers) {
  115. if (navigation_layers == p_navigation_layers) {
  116. return;
  117. }
  118. navigation_layers = p_navigation_layers;
  119. region_dirty = true;
  120. request_sync();
  121. }
  122. void NavRegion3D::set_enter_cost(real_t p_enter_cost) {
  123. real_t new_enter_cost = MAX(p_enter_cost, 0.0);
  124. if (enter_cost == new_enter_cost) {
  125. return;
  126. }
  127. enter_cost = new_enter_cost;
  128. region_dirty = true;
  129. request_sync();
  130. }
  131. void NavRegion3D::set_travel_cost(real_t p_travel_cost) {
  132. real_t new_travel_cost = MAX(p_travel_cost, 0.0);
  133. if (travel_cost == new_travel_cost) {
  134. return;
  135. }
  136. travel_cost = new_travel_cost;
  137. region_dirty = true;
  138. request_sync();
  139. }
  140. void NavRegion3D::set_owner_id(ObjectID p_owner_id) {
  141. if (owner_id == p_owner_id) {
  142. return;
  143. }
  144. owner_id = p_owner_id;
  145. region_dirty = true;
  146. request_sync();
  147. }
  148. bool NavRegion3D::sync() {
  149. RWLockWrite write_lock(region_rwlock);
  150. bool something_changed = region_dirty || polygons_dirty;
  151. region_dirty = false;
  152. update_polygons();
  153. if (something_changed) {
  154. iteration_id = iteration_id % UINT32_MAX + 1;
  155. }
  156. return something_changed;
  157. }
  158. void NavRegion3D::update_polygons() {
  159. if (!polygons_dirty) {
  160. return;
  161. }
  162. navmesh_polygons.clear();
  163. surface_area = 0.0;
  164. bounds = AABB();
  165. polygons_dirty = false;
  166. if (map == nullptr) {
  167. return;
  168. }
  169. RWLockRead read_lock(navmesh_rwlock);
  170. if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) {
  171. return;
  172. }
  173. int len = pending_navmesh_vertices.size();
  174. if (len == 0) {
  175. return;
  176. }
  177. const Vector3 *vertices_r = pending_navmesh_vertices.ptr();
  178. navmesh_polygons.resize(pending_navmesh_polygons.size());
  179. real_t _new_region_surface_area = 0.0;
  180. AABB _new_bounds;
  181. bool first_vertex = true;
  182. int navigation_mesh_polygon_index = 0;
  183. for (Polygon &polygon : navmesh_polygons) {
  184. polygon.surface_area = 0.0;
  185. Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
  186. navigation_mesh_polygon_index += 1;
  187. int navigation_mesh_polygon_size = navigation_mesh_polygon.size();
  188. if (navigation_mesh_polygon_size < 3) {
  189. continue;
  190. }
  191. const int *indices = navigation_mesh_polygon.ptr();
  192. bool valid(true);
  193. polygon.vertices.resize(navigation_mesh_polygon_size);
  194. polygon.edges.resize(navigation_mesh_polygon_size);
  195. real_t _new_polygon_surface_area = 0.0;
  196. for (int j(2); j < navigation_mesh_polygon_size; j++) {
  197. const Face3 face = Face3(
  198. transform.xform(vertices_r[indices[0]]),
  199. transform.xform(vertices_r[indices[j - 1]]),
  200. transform.xform(vertices_r[indices[j]]));
  201. _new_polygon_surface_area += face.get_area();
  202. }
  203. polygon.surface_area = _new_polygon_surface_area;
  204. _new_region_surface_area += _new_polygon_surface_area;
  205. for (int j(0); j < navigation_mesh_polygon_size; j++) {
  206. int idx = indices[j];
  207. if (idx < 0 || idx >= len) {
  208. valid = false;
  209. break;
  210. }
  211. Vector3 point_position = transform.xform(vertices_r[idx]);
  212. polygon.vertices[j] = point_position;
  213. if (first_vertex) {
  214. first_vertex = false;
  215. _new_bounds.position = point_position;
  216. } else {
  217. _new_bounds.expand_to(point_position);
  218. }
  219. }
  220. if (!valid) {
  221. ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!");
  222. }
  223. }
  224. surface_area = _new_region_surface_area;
  225. bounds = _new_bounds;
  226. }
  227. void NavRegion3D::get_iteration_update(NavRegionIteration3D &r_iteration) {
  228. r_iteration.navigation_layers = get_navigation_layers();
  229. r_iteration.enter_cost = get_enter_cost();
  230. r_iteration.travel_cost = get_travel_cost();
  231. r_iteration.owner_object_id = get_owner_id();
  232. r_iteration.owner_type = get_type();
  233. r_iteration.owner_rid = get_self();
  234. r_iteration.enabled = get_enabled();
  235. r_iteration.transform = get_transform();
  236. r_iteration.owner_use_edge_connections = get_use_edge_connections();
  237. r_iteration.bounds = get_bounds();
  238. r_iteration.surface_area = get_surface_area();
  239. r_iteration.navmesh_polygons.clear();
  240. r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
  241. for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
  242. Polygon &navmesh_polygon = navmesh_polygons[i];
  243. navmesh_polygon.owner = &r_iteration;
  244. r_iteration.navmesh_polygons[i] = navmesh_polygon;
  245. }
  246. }
  247. void NavRegion3D::request_sync() {
  248. if (map && !sync_dirty_request_list_element.in_list()) {
  249. map->add_region_sync_dirty_request(&sync_dirty_request_list_element);
  250. }
  251. }
  252. void NavRegion3D::cancel_sync_request() {
  253. if (map && sync_dirty_request_list_element.in_list()) {
  254. map->remove_region_sync_dirty_request(&sync_dirty_request_list_element);
  255. }
  256. }
  257. NavRegion3D::NavRegion3D() :
  258. sync_dirty_request_list_element(this) {
  259. type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
  260. }
  261. NavRegion3D::~NavRegion3D() {
  262. cancel_sync_request();
  263. }