summaryrefslogtreecommitdiff
path: root/modules
diff options
context:
space:
mode:
Diffstat (limited to 'modules')
-rw-r--r--modules/gdnavigation/gd_navigation_server.cpp36
-rw-r--r--modules/gdnavigation/gd_navigation_server.h5
-rw-r--r--modules/gdnavigation/nav_map.cpp138
-rw-r--r--modules/gdnavigation/nav_map.h4
4 files changed, 179 insertions, 4 deletions
diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
index 5bafa5507c..1f1783802d 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<Vector3> 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<Vector3>());
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<GdNavigationServer *>(this);
mut_this->operations_mutex->lock();
diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h
index 564e9870a0..7fa5979c31 100644
--- a/modules/gdnavigation/gd_navigation_server.h
+++ b/modules/gdnavigation/gd_navigation_server.h
@@ -103,6 +103,11 @@ public:
virtual Vector<Vector3> 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 d1765f4da9..d3e2f8f388 100644
--- a/modules/gdnavigation/nav_map.cpp
+++ b/modules/gdnavigation/nav_map.cpp
@@ -401,6 +401,144 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
return Vector<Vector3>();
}
+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 128a82580c..4543f00926 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<Vector3> 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);
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-23 04:25:52 +0000