summaryrefslogtreecommitdiff
path: root/modules/gdnavigation/nav_map.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'modules/gdnavigation/nav_map.cpp')
-rw-r--r--modules/gdnavigation/nav_map.cpp59
1 files changed, 16 insertions, 43 deletions
diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp
index 7e6a3f7a26..7919e6a01f 100644
--- a/modules/gdnavigation/nav_map.cpp
+++ b/modules/gdnavigation/nav_map.cpp
@@ -33,6 +33,7 @@
#include "core/os/threaded_array_processor.h"
#include "nav_region.h"
#include "rvo_agent.h"
+
#include <algorithm>
/**
@@ -41,16 +42,6 @@
#define USE_ENTRY_POINT
-NavMap::NavMap() :
- up(0, 1, 0),
- cell_size(0.3),
- edge_connection_margin(5.0),
- regenerate_polygons(true),
- regenerate_links(true),
- agents_dirty(false),
- deltatime(0.0),
- map_update_id(0) {}
-
void NavMap::set_up(Vector3 p_up) {
up = p_up;
regenerate_polygons = true;
@@ -80,7 +71,6 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
}
Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
-
const gd::Polygon *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr;
Vector3 begin_point;
@@ -94,7 +84,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// For each point cast a face and check the distance between the origin/destination
for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
-
Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 spoint = f.get_closest_point_to(p_origin);
float dpoint = spoint.distance_to(p_origin);
@@ -120,7 +109,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
}
if (begin_poly == end_poly) {
-
Vector<Vector3> path;
path.resize(2);
path.write[0] = begin_point;
@@ -151,15 +139,15 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
bool is_reachable = true;
while (found_route == false) {
-
{
// Takes the current least_cost_poly neighbors and compute the traveled_distance of each
for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
const gd::Edge &edge = least_cost_poly->poly->edges[i];
- if (!edge.other_polygon)
+ if (!edge.other_polygon) {
continue;
+ }
#ifdef USE_ENTRY_POINT
Vector3 edge_line[2] = {
@@ -167,7 +155,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos
};
- const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly->entry, edge_line);
+ const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, edge_line);
const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance;
#else
const float new_distance = least_cost_poly->poly->center.distance_to(edge.other_polygon->center) + least_cost_poly->traveled_distance;
@@ -181,7 +169,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
if (it != navigation_polys.end()) {
// Oh this was visited already, can we win the cost?
if (it->traveled_distance > new_distance) {
-
it->prev_navigation_poly_id = least_cost_id;
it->back_navigation_edge = edge.other_edge;
it->traveled_distance = new_distance;
@@ -283,10 +270,8 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
}
if (found_route) {
-
Vector<Vector3> path;
if (p_optimize) {
-
// String pulling
gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
@@ -300,7 +285,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
path.push_back(end_point);
while (p) {
-
Vector3 left;
Vector3 right;
@@ -315,7 +299,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
left = p->poly->points[prev].pos;
right = p->poly->points[prev_n].pos;
- //if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){
if (p->poly->clockwise) {
SWAP(left, right);
}
@@ -329,7 +312,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
left_poly = p;
portal_left = left;
} else {
-
clip_path(navigation_polys, path, apex_poly, portal_right, right_poly);
apex_point = portal_right;
@@ -349,7 +331,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
right_poly = p;
portal_right = right;
} else {
-
clip_path(navigation_polys, path, apex_poly, portal_left, left_poly);
apex_point = portal_left;
@@ -362,15 +343,17 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
}
}
- if (p->prev_navigation_poly_id != -1)
+ if (p->prev_navigation_poly_id != -1) {
p = &navigation_polys[p->prev_navigation_poly_id];
- else
+ } else {
// The end
p = nullptr;
+ }
}
- if (path[path.size() - 1] != begin_point)
+ if (path[path.size() - 1] != begin_point) {
path.push_back(begin_point);
+ }
path.invert();
@@ -380,7 +363,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// Add mid points
int np_id = least_cost_id;
while (np_id != -1) {
-
#ifdef USE_ENTRY_POINT
Vector3 point = navigation_polys[np_id].entry;
#else
@@ -402,7 +384,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
}
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;
@@ -413,7 +394,6 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
// 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)) {
@@ -423,7 +403,6 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
use_collision = true;
closest_point_d = d;
} else if (closest_point_d > d) {
-
closest_point = inters;
closest_point_d = d;
}
@@ -431,12 +410,10 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
}
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(
+ Geometry3D::get_closest_points_between_segments(
p_from,
p_to,
p.points[point_id].pos,
@@ -446,7 +423,6 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
const real_t d = a.distance_to(b);
if (d < closest_point_d) {
-
closest_point_d = d;
closest_point = b;
}
@@ -469,7 +445,6 @@ Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
// 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);
@@ -496,7 +471,6 @@ Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
// 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);
@@ -524,7 +498,6 @@ RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
// 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);
@@ -588,7 +561,6 @@ void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
}
void NavMap::sync() {
-
if (regenerate_polygons) {
for (size_t r(0); r < regions.size(); r++) {
regions[r]->scratch_polygons();
@@ -741,8 +713,9 @@ void NavMap::sync() {
if (agents_dirty) {
std::vector<RVO::Agent *> raw_agents;
raw_agents.reserve(agents.size());
- for (size_t i(0); i < agents.size(); i++)
+ for (size_t i(0); i < agents.size(); i++) {
raw_agents.push_back(agents[i]->get_agent());
+ }
rvo.buildAgentTree(raw_agents);
}
@@ -776,17 +749,18 @@ void NavMap::dispatch_callbacks() {
void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const {
Vector3 from = path[path.size() - 1];
- if (from.distance_to(p_to_point) < CMP_EPSILON)
+ if (from.distance_to(p_to_point) < CMP_EPSILON) {
return;
+ }
Plane cut_plane;
cut_plane.normal = (from - p_to_point).cross(up);
- if (cut_plane.normal == Vector3())
+ if (cut_plane.normal == Vector3()) {
return;
+ }
cut_plane.normal.normalize();
cut_plane.d = cut_plane.normal.dot(from);
while (from_poly != p_to_poly) {
-
int back_nav_edge = from_poly->back_navigation_edge;
Vector3 a = from_poly->poly->points[back_nav_edge].pos;
Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos;
@@ -795,7 +769,6 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys
from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id];
if (a.distance_to(b) > CMP_EPSILON) {
-
Vector3 inters;
if (cut_plane.intersects_segment(a, b, &inters)) {
if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-24 00:26:57 +0000