diff --git a/doc/classes/Navigation.xml b/doc/classes/Navigation.xml index d7da753c612..8057aca1275 100644 --- a/doc/classes/Navigation.xml +++ b/doc/classes/Navigation.xml @@ -15,7 +15,7 @@ - + @@ -28,6 +28,46 @@ Returns the path between two given points. Points are in local coordinate space. If [code]optimize[/code] is [code]true[/code] (the default), the agent properties associated with each [NavigationMesh] (radius, height, etc.) are considered in the path calculation, otherwise they are ignored. + + + + + + + + + + + Returns the closest point between the navigation surface and the segment. + + + + + + + + + Returns the point closest to the provided [code]point[/code] on the navigation mesh surface. + + + + + + + + + Returns the normal for the point returned by [method get_closest_point]. + + + + + + + + + Returns the owner region RID for the point returned by [method get_closest_point]. + + diff --git a/doc/classes/Navigation2D.xml b/doc/classes/Navigation2D.xml index 9ef4a51ff00..20c666106a1 100644 --- a/doc/classes/Navigation2D.xml +++ b/doc/classes/Navigation2D.xml @@ -15,7 +15,7 @@ - + @@ -28,6 +28,24 @@ Returns the path between two given points. Points are in local coordinate space. If [code]optimize[/code] is [code]true[/code] (the default), the path is smoothed by merging path segments where possible. + + + + + + + Returns the point closest to the provided [code]point[/code] on the navigation mesh surface. + + + + + + + + + Returns the owner region RID for the point returned by [method get_closest_point]. + + diff --git a/doc/classes/Navigation2DServer.xml b/doc/classes/Navigation2DServer.xml index 67619d708d8..22b5d2574e6 100644 --- a/doc/classes/Navigation2DServer.xml +++ b/doc/classes/Navigation2DServer.xml @@ -188,6 +188,28 @@ Returns the navigation path to reach the destination from the origin, while avoiding static obstacles. + + + + + + + + + Returns the point closest to the provided [code]point[/code] on the navigation mesh surface. + + + + + + + + + + + Returns the owner region RID for the point returned by [method map_get_closest_point]. + + diff --git a/doc/classes/NavigationServer.xml b/doc/classes/NavigationServer.xml index ad7c56f3613..61ea6f47b6c 100644 --- a/doc/classes/NavigationServer.xml +++ b/doc/classes/NavigationServer.xml @@ -188,6 +188,54 @@ Returns the navigation path to reach the destination from the origin. + + + + + + + + + + + + + Returns the closest point between the navigation surface and the segment. + + + + + + + + + + + Returns the point closest to the provided [code]point[/code] on the navigation mesh surface. + + + + + + + + + + + Returns the normal for the point returned by [method map_get_closest_point]. + + + + + + + + + + + Returns the owner region RID for the point returned by [method map_get_closest_point]. + + diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp index 5bafa5507cd..1f1783802de 100644 --- a/modules/gdnavigation/gd_navigation_server.cpp +++ b/modules/gdnavigation/gd_navigation_server.cpp @@ -170,7 +170,7 @@ COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) { } Vector3 GdNavigationServer::map_get_up(RID p_map) const { - NavMap *map = map_owner.getornull(p_map); + const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == NULL, Vector3()); return map->get_up(); @@ -184,7 +184,7 @@ COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) { } real_t GdNavigationServer::map_get_cell_size(RID p_map) const { - NavMap *map = map_owner.getornull(p_map); + const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == NULL, 0); return map->get_cell_size(); @@ -198,19 +198,47 @@ COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margi } real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const { - NavMap *map = map_owner.getornull(p_map); + const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == NULL, 0); return map->get_edge_connection_margin(); } Vector GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const { - NavMap *map = map_owner.getornull(p_map); + const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == NULL, Vector()); return map->get_path(p_origin, p_destination, p_optimize); } +Vector3 GdNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == NULL, Vector3()); + + return map->get_closest_point_to_segment(p_from, p_to, p_use_collision); +} + +Vector3 GdNavigationServer::map_get_closest_point(RID p_map, const Vector3 &p_point) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == NULL, Vector3()); + + return map->get_closest_point(p_point); +} + +Vector3 GdNavigationServer::map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == NULL, Vector3()); + + return map->get_closest_point_normal(p_point); +} + +RID GdNavigationServer::map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == NULL, RID()); + + return map->get_closest_point_owner(p_point); +} + RID GdNavigationServer::region_create() const { auto mut_this = const_cast(this); mut_this->operations_mutex->lock(); diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h index 564e9870a01..7fa5979c312 100644 --- a/modules/gdnavigation/gd_navigation_server.h +++ b/modules/gdnavigation/gd_navigation_server.h @@ -103,6 +103,11 @@ public: virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const; + virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const; + virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const; + virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const; + virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const; + virtual RID region_create() const; COMMAND_2(region_set_map, RID, p_region, RID, p_map); COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform); diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp index d1765f4da94..d3e2f8f3881 100644 --- a/modules/gdnavigation/nav_map.cpp +++ b/modules/gdnavigation/nav_map.cpp @@ -401,6 +401,144 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p return Vector(); } +Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { + + bool use_collision = p_use_collision; + Vector3 closest_point; + real_t closest_point_d = 1e20; + + // Find the initial poly and the end poly on this map. + for (size_t i(0); i < polygons.size(); i++) { + const gd::Polygon &p = polygons[i]; + + // For each point cast a face and check the distance to the segment + for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { + + const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + Vector3 inters; + if (f.intersects_segment(p_from, p_to, &inters)) { + const real_t d = closest_point_d = p_from.distance_to(inters); + if (use_collision == false) { + closest_point = inters; + use_collision = true; + closest_point_d = d; + } else if (closest_point_d > d) { + + closest_point = inters; + closest_point_d = d; + } + } + } + + if (use_collision == false) { + + for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) { + + Vector3 a, b; + + Geometry::get_closest_points_between_segments( + p_from, + p_to, + p.points[point_id].pos, + p.points[(point_id + 1) % p.points.size()].pos, + a, + b); + + const real_t d = a.distance_to(b); + if (d < closest_point_d) { + + closest_point_d = d; + closest_point = b; + } + } + } + } + + return closest_point; +} + +Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { + // TODO this is really not optimal, please redesign the API to directly return all this data + + Vector3 closest_point; + real_t closest_point_d = 1e20; + + // Find the initial poly and the end poly on this map. + for (size_t i(0); i < polygons.size(); i++) { + const gd::Polygon &p = polygons[i]; + + // For each point cast a face and check the distance to the point + for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { + + const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + const Vector3 inters = f.get_closest_point_to(p_point); + const real_t d = inters.distance_to(p_point); + if (d < closest_point_d) { + closest_point = inters; + closest_point_d = d; + } + } + } + + return closest_point; +} + +Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { + // TODO this is really not optimal, please redesign the API to directly return all this data + + Vector3 closest_point; + Vector3 closest_point_normal; + real_t closest_point_d = 1e20; + + // Find the initial poly and the end poly on this map. + for (size_t i(0); i < polygons.size(); i++) { + const gd::Polygon &p = polygons[i]; + + // For each point cast a face and check the distance to the point + for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { + + const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + const Vector3 inters = f.get_closest_point_to(p_point); + const real_t d = inters.distance_to(p_point); + if (d < closest_point_d) { + closest_point = inters; + closest_point_normal = f.get_plane().normal; + closest_point_d = d; + } + } + } + + return closest_point_normal; +} + +RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { + // TODO this is really not optimal, please redesign the API to directly return all this data + + Vector3 closest_point; + RID closest_point_owner; + real_t closest_point_d = 1e20; + + // Find the initial poly and the end poly on this map. + for (size_t i(0); i < polygons.size(); i++) { + const gd::Polygon &p = polygons[i]; + + // For each point cast a face and check the distance to the point + for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { + + const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + const Vector3 inters = f.get_closest_point_to(p_point); + const real_t d = inters.distance_to(p_point); + if (d < closest_point_d) { + closest_point = inters; + closest_point_owner = p.owner->get_self(); + closest_point_d = d; + } + } + } + + return closest_point_owner; +} + void NavMap::add_region(NavRegion *p_region) { regions.push_back(p_region); regenerate_links = true; diff --git a/modules/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h index 128a82580cf..4543f009268 100644 --- a/modules/gdnavigation/nav_map.h +++ b/modules/gdnavigation/nav_map.h @@ -104,6 +104,10 @@ public: gd::PointKey get_point_key(const Vector3 &p_pos) const; Vector get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const; + Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const; + Vector3 get_closest_point(const Vector3 &p_point) const; + Vector3 get_closest_point_normal(const Vector3 &p_point) const; + RID get_closest_point_owner(const Vector3 &p_point) const; void add_region(NavRegion *p_region); void remove_region(NavRegion *p_region); diff --git a/scene/2d/navigation_2d.cpp b/scene/2d/navigation_2d.cpp index de01d97ad96..45574d4a5ad 100644 --- a/scene/2d/navigation_2d.cpp +++ b/scene/2d/navigation_2d.cpp @@ -35,6 +35,8 @@ void Navigation2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_rid"), &Navigation2D::get_rid); ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation2D::get_closest_point); + ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner); ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation2D::set_cell_size); ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation2D::get_cell_size); @@ -68,10 +70,18 @@ void Navigation2D::set_edge_connection_margin(float p_edge_connection_margin) { Navigation2DServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin); } -Vector Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) { +Vector Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) const { return Navigation2DServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize); } +Vector2 Navigation2D::get_closest_point(const Vector2 &p_point) const { + return Navigation2DServer::get_singleton()->map_get_closest_point(map, p_point); +} + +RID Navigation2D::get_closest_point_owner(const Vector2 &p_point) const { + return Navigation2DServer::get_singleton()->map_get_closest_point_owner(map, p_point); +} + Navigation2D::Navigation2D() { map = Navigation2DServer::get_singleton()->map_create(); diff --git a/scene/2d/navigation_2d.h b/scene/2d/navigation_2d.h index 08642a5489a..16e20d8f9bb 100644 --- a/scene/2d/navigation_2d.h +++ b/scene/2d/navigation_2d.h @@ -61,7 +61,9 @@ public: return edge_connection_margin; } - Vector get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize = true); + Vector get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize = true) const; + Vector2 get_closest_point(const Vector2 &p_point) const; + RID get_closest_point_owner(const Vector2 &p_point) const; Navigation2D(); }; diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp index 10b12f5c75f..f2109a2f330 100644 --- a/scene/3d/navigation.cpp +++ b/scene/3d/navigation.cpp @@ -32,11 +32,27 @@ #include "servers/navigation_server.h" -Vector Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) { +Vector Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) const { return NavigationServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize); } +Vector3 Navigation::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const { + return NavigationServer::get_singleton()->map_get_closest_point_to_segment(map, p_from, p_to, p_use_collision); +} + +Vector3 Navigation::get_closest_point(const Vector3 &p_point) const { + return NavigationServer::get_singleton()->map_get_closest_point(map, p_point); +} + +Vector3 Navigation::get_closest_point_normal(const Vector3 &p_point) const { + return NavigationServer::get_singleton()->map_get_closest_point_normal(map, p_point); +} + +RID Navigation::get_closest_point_owner(const Vector3 &p_point) const { + return NavigationServer::get_singleton()->map_get_closest_point_owner(map, p_point); +} + void Navigation::set_up_vector(const Vector3 &p_up) { up = p_up; @@ -63,6 +79,10 @@ void Navigation::_bind_methods() { ClassDB::bind_method(D_METHOD("get_rid"), &Navigation::get_rid); ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation::get_simple_path, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("get_closest_point_to_segment", "start", "end", "use_collision"), &Navigation::get_closest_point_to_segment, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation::get_closest_point); + ClassDB::bind_method(D_METHOD("get_closest_point_normal", "to_point"), &Navigation::get_closest_point_normal); + ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation::get_closest_point_owner); ClassDB::bind_method(D_METHOD("set_up_vector", "up"), &Navigation::set_up_vector); ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector); diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h index 68e041ad733..85887651fff 100644 --- a/scene/3d/navigation.h +++ b/scene/3d/navigation.h @@ -66,7 +66,11 @@ public: return edge_connection_margin; } - Vector get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize = true); + Vector get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize = true) const; + Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision = false) const; + Vector3 get_closest_point(const Vector3 &p_point) const; + Vector3 get_closest_point_normal(const Vector3 &p_point) const; + RID get_closest_point_owner(const Vector3 &p_point) const; Navigation(); ~Navigation(); diff --git a/servers/navigation_2d_server.cpp b/servers/navigation_2d_server.cpp index 94ddecf9c39..d9b53122e2b 100644 --- a/servers/navigation_2d_server.cpp +++ b/servers/navigation_2d_server.cpp @@ -62,6 +62,12 @@ Navigation2DServer *Navigation2DServer::singleton = NULL; return NavigationServer::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \ } +#define FORWARD_2_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \ + Navigation2DServer::FUNC_NAME(T_0 D_0, T_1 D_1) \ + const { \ + return CONV_R(NavigationServer::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \ + } + #define FORWARD_4_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, CONV_0, CONV_1, CONV_2, CONV_3) \ Navigation2DServer::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) \ const { \ @@ -132,6 +138,8 @@ void Navigation2DServer::_bind_methods() { ClassDB::bind_method(D_METHOD("map_set_edge_connection_margin", "map", "margin"), &Navigation2DServer::map_set_edge_connection_margin); ClassDB::bind_method(D_METHOD("map_get_edge_connection_margin", "map"), &Navigation2DServer::map_get_edge_connection_margin); ClassDB::bind_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize"), &Navigation2DServer::map_get_path); + ClassDB::bind_method(D_METHOD("map_get_closest_point", "map", "to_point"), &Navigation2DServer::map_get_closest_point); + ClassDB::bind_method(D_METHOD("map_get_closest_point_owner", "map", "to_point"), &Navigation2DServer::map_get_closest_point_owner); ClassDB::bind_method(D_METHOD("region_create"), &Navigation2DServer::region_create); ClassDB::bind_method(D_METHOD("region_set_map", "region", "map"), &Navigation2DServer::region_set_map); @@ -176,6 +184,9 @@ real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid); Vector FORWARD_4_R_C(vector_v3_to_v2, map_get_path, RID, p_map, Vector2, p_origin, Vector2, p_destination, bool, p_optimize, rid_to_rid, v2_to_v3, v2_to_v3, bool_to_bool); +Vector2 FORWARD_2_R_C(v3_to_v2, map_get_closest_point, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3); +RID FORWARD_2_C(map_get_closest_point_owner, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3); + RID FORWARD_0_C(region_create); void FORWARD_2_C(region_set_map, RID, p_region, RID, p_map, rid_to_rid, rid_to_rid); diff --git a/servers/navigation_2d_server.h b/servers/navigation_2d_server.h index 2ac0e8f8751..955b0c3726a 100644 --- a/servers/navigation_2d_server.h +++ b/servers/navigation_2d_server.h @@ -79,6 +79,9 @@ public: /// Returns the navigation path to reach the destination from the origin. virtual Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize) const; + virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const; + virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const; + /// Creates a new region. virtual RID region_create() const; diff --git a/servers/navigation_server.cpp b/servers/navigation_server.cpp index d28aed91106..f31795fb357 100644 --- a/servers/navigation_server.cpp +++ b/servers/navigation_server.cpp @@ -48,6 +48,10 @@ void NavigationServer::_bind_methods() { ClassDB::bind_method(D_METHOD("map_set_edge_connection_margin", "map", "margin"), &NavigationServer::map_set_edge_connection_margin); ClassDB::bind_method(D_METHOD("map_get_edge_connection_margin", "map"), &NavigationServer::map_get_edge_connection_margin); ClassDB::bind_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize"), &NavigationServer::map_get_path); + ClassDB::bind_method(D_METHOD("map_get_closest_point_to_segment", "map", "start", "end", "use_collision"), &NavigationServer::map_get_closest_point_to_segment, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("map_get_closest_point", "map", "to_point"), &NavigationServer::map_get_closest_point); + ClassDB::bind_method(D_METHOD("map_get_closest_point_normal", "map", "to_point"), &NavigationServer::map_get_closest_point_normal); + ClassDB::bind_method(D_METHOD("map_get_closest_point_owner", "map", "to_point"), &NavigationServer::map_get_closest_point_owner); ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer::region_create); ClassDB::bind_method(D_METHOD("region_set_map", "region", "map"), &NavigationServer::region_set_map); diff --git a/servers/navigation_server.h b/servers/navigation_server.h index bcdbf843396..d4d95d72d42 100644 --- a/servers/navigation_server.h +++ b/servers/navigation_server.h @@ -90,6 +90,11 @@ public: /// Returns the navigation path to reach the destination from the origin. virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const = 0; + virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const = 0; + virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const = 0; + virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const = 0; + virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const = 0; + /// Creates a new region. virtual RID region_create() const = 0;