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.cpp526
1 files changed, 255 insertions, 271 deletions
diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp
index 2646a4cc0c..41306f0687 100644
--- a/modules/gdnavigation/nav_map.cpp
+++ b/modules/gdnavigation/nav_map.cpp
@@ -40,7 +40,7 @@
@author AndreaCatania
*/
-#define USE_ENTRY_POINT
+#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
void NavMap::set_up(Vector3 p_up) {
up = p_up;
@@ -70,44 +70,52 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
return p;
}
-Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
+Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
+ // Find the start poly and the end poly on this map.
const gd::Polygon *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr;
Vector3 begin_point;
Vector3 end_point;
float begin_d = 1e20;
float end_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];
+ // Only consider the polygon if it in a region with compatible layers.
+ if ((p_layers & p.owner->get_layers()) == 0) {
+ continue;
+ }
+
// 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);
- if (dpoint < begin_d) {
- begin_d = dpoint;
+ for (size_t point_id = 0; point_id < p.points.size(); point_id++) {
+ const Vector3 p1 = p.points[point_id].pos;
+ const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
+ const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
+ const Face3 face(p1, p2, p3);
+
+ Vector3 point = face.get_closest_point_to(p_origin);
+ float distance_to_point = point.distance_to(p_origin);
+ if (distance_to_point < begin_d) {
+ begin_d = distance_to_point;
begin_poly = &p;
- begin_point = spoint;
+ begin_point = point;
}
- spoint = f.get_closest_point_to(p_destination);
- dpoint = spoint.distance_to(p_destination);
- if (dpoint < end_d) {
- end_d = dpoint;
+ point = face.get_closest_point_to(p_destination);
+ distance_to_point = point.distance_to(p_destination);
+ if (distance_to_point < end_d) {
+ end_d = distance_to_point;
end_poly = &p;
- end_point = spoint;
+ end_point = point;
}
}
}
+ // Check for trivial cases
if (!begin_poly || !end_poly) {
- // No path
return Vector<Vector3>();
}
-
if (begin_poly == end_poly) {
Vector<Vector3> path;
path.resize(2);
@@ -116,90 +124,89 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
return path;
}
+ // List of all reachable navigation polys.
std::vector<gd::NavigationPoly> navigation_polys;
navigation_polys.reserve(polygons.size() * 0.75);
- // The elements indices in the `navigation_polys`.
- int least_cost_id(-1);
- List<uint32_t> open_list;
- bool found_route = false;
+ // Add the start polygon to the reachable navigation polygons.
+ gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly);
+ begin_navigation_poly.self_id = 0;
+ begin_navigation_poly.entry = begin_point;
+ begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
+ begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
+ navigation_polys.push_back(begin_navigation_poly);
- navigation_polys.push_back(gd::NavigationPoly(begin_poly));
- {
- least_cost_id = 0;
- gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
- least_cost_poly->self_id = least_cost_id;
- least_cost_poly->entry = begin_point;
- }
+ // List of polygon IDs to visit.
+ List<uint32_t> to_visit;
+ to_visit.push_back(0);
- open_list.push_back(0);
+ // This is an implementation of the A* algorithm.
+ int least_cost_id = 0;
+ bool found_route = false;
const gd::Polygon *reachable_end = nullptr;
float reachable_d = 1e30;
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];
+ while (true) {
+ gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+
+ // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
+ for (size_t i = 0; i < least_cost_poly->poly->edges.size(); i++) {
+ const gd::Edge &edge = least_cost_poly->poly->edges[i];
+
+ // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
+ for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
+ const gd::Edge::Connection &connection = edge.connections[connection_index];
- const gd::Edge &edge = least_cost_poly->poly->edges[i];
- if (!edge.other_polygon) {
+ // Only consider the connection to another polygon if this polygon is in a region with compatible layers.
+ if ((p_layers & connection.polygon->owner->get_layers()) == 0) {
continue;
}
-#ifdef USE_ENTRY_POINT
- Vector3 edge_line[2] = {
- least_cost_poly->poly->points[i].pos,
- least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos
- };
-
- const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, edge_line);
+ Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
+ const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, pathway);
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;
-#endif
- auto it = std::find(
+ const std::vector<gd::NavigationPoly>::iterator it = std::find(
navigation_polys.begin(),
navigation_polys.end(),
- gd::NavigationPoly(edge.other_polygon));
+ gd::NavigationPoly(connection.polygon));
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;
+ // Polygon already visited, check if we can reduce the travel cost.
+ if (new_distance < it->traveled_distance) {
+ it->back_navigation_poly_id = least_cost_id;
+ it->back_navigation_edge = connection.edge;
+ it->back_navigation_edge_pathway_start = connection.pathway_start;
+ it->back_navigation_edge_pathway_end = connection.pathway_end;
it->traveled_distance = new_distance;
-#ifdef USE_ENTRY_POINT
it->entry = new_entry;
-#endif
}
} else {
- // Add to open neighbours
-
- navigation_polys.push_back(gd::NavigationPoly(edge.other_polygon));
- gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1];
-
- np->self_id = navigation_polys.size() - 1;
- np->prev_navigation_poly_id = least_cost_id;
- np->back_navigation_edge = edge.other_edge;
- np->traveled_distance = new_distance;
-#ifdef USE_ENTRY_POINT
- np->entry = new_entry;
-#endif
- open_list.push_back(navigation_polys.size() - 1);
+ // Add the neighbour polygon to the reachable ones.
+ gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon);
+ new_navigation_poly.self_id = navigation_polys.size();
+ new_navigation_poly.back_navigation_poly_id = least_cost_id;
+ new_navigation_poly.back_navigation_edge = connection.edge;
+ new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start;
+ new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end;
+ new_navigation_poly.traveled_distance = new_distance;
+ new_navigation_poly.entry = new_entry;
+ navigation_polys.push_back(new_navigation_poly);
+
+ // Add the neighbour polygon to the polygons to visit.
+ to_visit.push_back(navigation_polys.size() - 1);
}
}
}
- // Removes the least cost polygon from the open list so we can advance.
- open_list.erase(least_cost_id);
+ // Removes the least cost polygon from the list of polygons to visit so we can advance.
+ to_visit.erase(least_cost_id);
- if (open_list.size() == 0) {
- // When the open list is empty at this point the End Polygon is not reachable
- // so use the further reachable polygon
+ // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable
+ if (to_visit.size() == 0) {
+ // Thus use the further reachable polygon
ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
is_reachable = false;
if (reachable_end == nullptr) {
@@ -224,26 +231,21 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
gd::NavigationPoly np = navigation_polys[0];
navigation_polys.clear();
navigation_polys.push_back(np);
- open_list.clear();
- open_list.push_back(0);
+ to_visit.clear();
+ to_visit.push_back(0);
reachable_end = nullptr;
continue;
}
- // Now take the new least_cost_poly from the open list.
+ // Find the polygon with the minimum cost from the list of polygons to visit.
least_cost_id = -1;
float least_cost = 1e30;
-
- for (auto element = open_list.front(); element != nullptr; element = element->next()) {
+ for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
gd::NavigationPoly *np = &navigation_polys[element->get()];
float cost = np->traveled_distance;
-#ifdef USE_ENTRY_POINT
cost += np->entry.distance_to(end_point);
-#else
- cost += np->poly->center.distance_to(end_point);
-#endif
if (cost < least_cost) {
least_cost_id = np->self_id;
least_cost = cost;
@@ -263,124 +265,108 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// Check if we reached the end
if (navigation_polys[least_cost_id].poly == end_poly) {
- // Yep, done!!
found_route = true;
break;
}
}
- if (found_route) {
- Vector<Vector3> path;
- if (p_optimize) {
- // String pulling
-
- gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
- Vector3 apex_point = end_point;
- Vector3 portal_left = apex_point;
- Vector3 portal_right = apex_point;
- gd::NavigationPoly *left_poly = apex_poly;
- gd::NavigationPoly *right_poly = apex_poly;
- gd::NavigationPoly *p = apex_poly;
-
- path.push_back(end_point);
+ // If we did not find a route, return an empty path.
+ if (!found_route) {
+ return Vector<Vector3>();
+ }
- while (p) {
- Vector3 left;
- Vector3 right;
+ Vector<Vector3> path;
+ // Optimize the path.
+ if (p_optimize) {
+ // Set the apex poly/point to the end point
+ gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+ Vector3 apex_point = end_point;
-#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b)))
+ gd::NavigationPoly *left_poly = apex_poly;
+ Vector3 left_portal = apex_point;
+ gd::NavigationPoly *right_poly = apex_poly;
+ Vector3 right_portal = apex_point;
- if (p->poly == begin_poly) {
- left = begin_point;
- right = begin_point;
- } else {
- int prev = p->back_navigation_edge;
- int prev_n = (p->back_navigation_edge + 1) % p->poly->points.size();
- left = p->poly->points[prev].pos;
- right = p->poly->points[prev_n].pos;
+ gd::NavigationPoly *p = apex_poly;
- if (p->poly->clockwise) {
- SWAP(left, right);
- }
- }
+ path.push_back(end_point);
- bool skip = false;
-
- if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) {
- //process
- if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) {
- left_poly = p;
- portal_left = left;
- } else {
- clip_path(navigation_polys, path, apex_poly, portal_right, right_poly);
-
- apex_point = portal_right;
- p = right_poly;
- left_poly = p;
- apex_poly = p;
- portal_left = apex_point;
- portal_right = apex_point;
- path.push_back(apex_point);
- skip = true;
- }
- }
+ while (p) {
+ // Set left and right points of the pathway between polygons.
+ Vector3 left = p->back_navigation_edge_pathway_start;
+ Vector3 right = p->back_navigation_edge_pathway_end;
+ if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) {
+ SWAP(left, right);
+ }
- if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) {
- //process
- if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) {
- right_poly = p;
- portal_right = right;
- } else {
- clip_path(navigation_polys, path, apex_poly, portal_left, left_poly);
-
- apex_point = portal_left;
- p = left_poly;
- right_poly = p;
- apex_poly = p;
- portal_right = apex_point;
- portal_left = apex_point;
- path.push_back(apex_point);
- }
+ bool skip = false;
+ if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) {
+ //process
+ if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) {
+ left_poly = p;
+ left_portal = left;
+ } else {
+ clip_path(navigation_polys, path, apex_poly, right_portal, right_poly);
+
+ apex_point = right_portal;
+ p = right_poly;
+ left_poly = p;
+ apex_poly = p;
+ left_portal = apex_point;
+ right_portal = apex_point;
+ path.push_back(apex_point);
+ skip = true;
}
+ }
- if (p->prev_navigation_poly_id != -1) {
- p = &navigation_polys[p->prev_navigation_poly_id];
+ if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) {
+ //process
+ if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) {
+ right_poly = p;
+ right_portal = right;
} else {
- // The end
- p = nullptr;
+ clip_path(navigation_polys, path, apex_poly, left_portal, left_poly);
+
+ apex_point = left_portal;
+ p = left_poly;
+ right_poly = p;
+ apex_poly = p;
+ right_portal = apex_point;
+ left_portal = apex_point;
+ path.push_back(apex_point);
}
}
- if (path[path.size() - 1] != begin_point) {
- path.push_back(begin_point);
+ // Go to the previous polygon.
+ if (p->back_navigation_poly_id != -1) {
+ p = &navigation_polys[p->back_navigation_poly_id];
+ } else {
+ // The end
+ p = nullptr;
}
+ }
- path.invert();
-
- } else {
- path.push_back(end_point);
+ // If the last point is not the begin point, add it to the list.
+ if (path[path.size() - 1] != begin_point) {
+ path.push_back(begin_point);
+ }
- // 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
- int prev = navigation_polys[np_id].back_navigation_edge;
- int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
- Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
-#endif
+ path.reverse();
- path.push_back(point);
- np_id = navigation_polys[np_id].prev_navigation_poly_id;
- }
+ } else {
+ path.push_back(end_point);
- path.invert();
+ // Add mid points
+ int np_id = least_cost_id;
+ while (np_id != -1) {
+ path.push_back(navigation_polys[np_id].entry);
+ np_id = navigation_polys[np_id].back_navigation_poly_id;
}
- return path;
+ path.reverse();
}
- return Vector<Vector3>();
+
+ return path;
}
Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@@ -518,7 +504,7 @@ void NavMap::add_region(NavRegion *p_region) {
}
void NavMap::remove_region(NavRegion *p_region) {
- std::vector<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region);
+ const std::vector<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region);
if (it != regions.end()) {
regions.erase(it);
regenerate_links = true;
@@ -538,7 +524,7 @@ void NavMap::add_agent(RvoAgent *agent) {
void NavMap::remove_agent(RvoAgent *agent) {
remove_agent_as_controlled(agent);
- auto it = std::find(agents.begin(), agents.end(), agent);
+ const std::vector<RvoAgent *>::iterator it = std::find(agents.begin(), agents.end(), agent);
if (it != agents.end()) {
agents.erase(it);
agents_dirty = true;
@@ -554,13 +540,14 @@ void NavMap::set_agent_as_controlled(RvoAgent *agent) {
}
void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
- auto it = std::find(controlled_agents.begin(), controlled_agents.end(), agent);
+ const std::vector<RvoAgent *>::iterator it = std::find(controlled_agents.begin(), controlled_agents.end(), agent);
if (it != controlled_agents.end()) {
controlled_agents.erase(it);
}
}
void NavMap::sync() {
+ // Check if we need to update the links.
if (regenerate_polygons) {
for (size_t r(0); r < regions.size(); r++) {
regions[r]->scratch_polygons();
@@ -575,27 +562,30 @@ void NavMap::sync() {
}
if (regenerate_links) {
- // Copy all region polygons in the map.
+ // Remove regions connections.
+ for (size_t r(0); r < regions.size(); r++) {
+ regions[r]->get_connections().clear();
+ }
+
+ // Resize the polygon count.
int count = 0;
for (size_t r(0); r < regions.size(); r++) {
count += regions[r]->get_polygons().size();
}
-
polygons.resize(count);
- count = 0;
+ // Copy all region polygons in the map.
+ count = 0;
for (size_t r(0); r < regions.size(); r++) {
std::copy(
regions[r]->get_polygons().data(),
regions[r]->get_polygons().data() + regions[r]->get_polygons().size(),
polygons.begin() + count);
-
count += regions[r]->get_polygons().size();
}
- // Connects the `Edges` of all the `Polygons` of all `Regions` each other.
- Map<gd::EdgeKey, gd::Connection> connections;
-
+ // Group all edges per key.
+ Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections;
for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) {
gd::Polygon &poly(polygons[poly_id]);
@@ -603,69 +593,40 @@ void NavMap::sync() {
int next_point = (p + 1) % poly.points.size();
gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
- Map<gd::EdgeKey, gd::Connection>::Element *connection = connections.find(ek);
+ Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *connection = connections.find(ek);
if (!connection) {
- // Nothing yet
- gd::Connection c;
- c.A = &poly;
- c.A_edge = p;
- c.B = nullptr;
- c.B_edge = -1;
- connections[ek] = c;
-
- } else if (connection->get().B == nullptr) {
- CRASH_COND(connection->get().A == nullptr); // Unreachable
-
- // Connect the two Polygons by this edge
- connection->get().B = &poly;
- connection->get().B_edge = p;
-
- connection->get().A->edges[connection->get().A_edge].this_edge = connection->get().A_edge;
- connection->get().A->edges[connection->get().A_edge].other_polygon = connection->get().B;
- connection->get().A->edges[connection->get().A_edge].other_edge = connection->get().B_edge;
-
- connection->get().B->edges[connection->get().B_edge].this_edge = connection->get().B_edge;
- connection->get().B->edges[connection->get().B_edge].other_polygon = connection->get().A;
- connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge;
+ connections[ek] = Vector<gd::Edge::Connection>();
+ }
+ if (connections[ek].size() <= 1) {
+ // Add the polygon/edge tuple to this key.
+ gd::Edge::Connection new_connection;
+ new_connection.polygon = &poly;
+ new_connection.edge = p;
+ new_connection.pathway_start = poly.points[p].pos;
+ new_connection.pathway_end = poly.points[next_point].pos;
+ connections[ek].push_back(new_connection);
} else {
// The edge is already connected with another edge, skip.
- ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the Navigation3D's `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
+ ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
}
}
}
- // Takes all the free edges.
- std::vector<gd::FreeEdge> free_edges;
- free_edges.reserve(connections.size());
-
- for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) {
- if (connection_element->get().B == nullptr) {
- CRASH_COND(connection_element->get().A == nullptr); // Unreachable
- CRASH_COND(connection_element->get().A_edge < 0); // Unreachable
-
- // This is a free edge
- uint32_t id(free_edges.size());
- free_edges.push_back(gd::FreeEdge());
- free_edges[id].is_free = true;
- free_edges[id].poly = connection_element->get().A;
- free_edges[id].edge_id = connection_element->get().A_edge;
- uint32_t point_0(free_edges[id].edge_id);
- uint32_t point_1((free_edges[id].edge_id + 1) % free_edges[id].poly->points.size());
- Vector3 pos_0 = free_edges[id].poly->points[point_0].pos;
- Vector3 pos_1 = free_edges[id].poly->points[point_1].pos;
- Vector3 relative = pos_1 - pos_0;
- free_edges[id].edge_center = (pos_0 + pos_1) / 2.0;
- free_edges[id].edge_dir = relative.normalized();
- free_edges[id].edge_len_squared = relative.length_squared();
+ Vector<gd::Edge::Connection> free_edges;
+ for (Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *E = connections.front(); E; E = E->next()) {
+ if (E->get().size() == 2) {
+ // Connect edge that are shared in different polygons.
+ gd::Edge::Connection &c1 = E->get().write[0];
+ gd::Edge::Connection &c2 = E->get().write[1];
+ c1.polygon->edges[c1.edge].connections.push_back(c2);
+ c2.polygon->edges[c2.edge].connections.push_back(c1);
+ // Note: The pathway_start/end are full for those connection and do not need to be modified.
+ } else {
+ CRASH_COND_MSG(E->get().size() != 1, vformat("Number of connection != 1. Found: %d", E->get().size()));
+ free_edges.push_back(E->get()[0]);
}
}
- const float ecm_squared(edge_connection_margin * edge_connection_margin);
-#define LEN_TOLLERANCE 0.1
-#define DIR_TOLLERANCE 0.9
- // In front of tolerance
-#define IFO_TOLLERANCE 0.5
-
// Find the compatible near edges.
//
// Note:
@@ -673,43 +634,67 @@ void NavMap::sync() {
// to be connected, create new polygons to remove that small gap is
// not really useful and would result in wasteful computation during
// connection, integration and path finding.
- for (size_t i(0); i < free_edges.size(); i++) {
- if (!free_edges[i].is_free) {
- continue;
- }
- gd::FreeEdge &edge = free_edges[i];
- for (size_t y(0); y < free_edges.size(); y++) {
- gd::FreeEdge &other_edge = free_edges[y];
- if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) {
+ for (int i = 0; i < free_edges.size(); i++) {
+ const gd::Edge::Connection &free_edge = free_edges[i];
+ Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
+ Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
+
+ for (int j = 0; j < free_edges.size(); j++) {
+ const gd::Edge::Connection &other_edge = free_edges[j];
+ if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
continue;
}
- Vector3 rel_centers = other_edge.edge_center - edge.edge_center;
- if (ecm_squared > rel_centers.length_squared() // Are enough closer?
- && ABS(edge.edge_len_squared - other_edge.edge_len_squared) < LEN_TOLLERANCE // Are the same length?
- && ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLLERANCE // Are aligned?
- && ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLLERANCE // Are one in front the other?
- ) {
- // The edges can be connected
- edge.is_free = false;
- other_edge.is_free = false;
-
- edge.poly->edges[edge.edge_id].this_edge = edge.edge_id;
- edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id;
- edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly;
-
- other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id;
- other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id;
- other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly;
+ Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
+ Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
+
+ // Compute the projection of the opposite edge on the current one
+ Vector3 edge_vector = edge_p2 - edge_p1;
+ float projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
+ float projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
+ if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
+ continue;
+ }
+
+ // Check if the two edges are close to each other enough and compute a pathway between the two regions.
+ Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+ Vector3 other1;
+ if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
+ other1 = other_edge_p1;
+ } else {
+ other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
}
+ if ((self1 - other1).length() > edge_connection_margin) {
+ continue;
+ }
+
+ Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+ Vector3 other2;
+ if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
+ other2 = other_edge_p2;
+ } else {
+ other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+ }
+ if ((self2 - other2).length() > edge_connection_margin) {
+ continue;
+ }
+
+ // The edges can now be connected.
+ gd::Edge::Connection new_connection = other_edge;
+ new_connection.pathway_start = (self1 + other1) / 2.0;
+ new_connection.pathway_end = (self2 + other2) / 2.0;
+ free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
+
+ // Add the connection to the region_connection map.
+ free_edge.polygon->owner->get_connections().push_back(new_connection);
}
}
- }
- if (regenerate_links) {
+ // Update the update ID.
map_update_id = (map_update_id + 1) % 9999999;
}
+ // Update agents tree.
if (agents_dirty) {
std::vector<RVO::Agent *> raw_agents;
raw_agents.reserve(agents.size());
@@ -761,16 +746,15 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys
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;
+ Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start;
+ Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
- ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1);
- from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id];
+ ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
+ from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
- if (a.distance_to(b) > CMP_EPSILON) {
+ if (pathway_start.distance_to(pathway_end) > CMP_EPSILON) {
Vector3 inters;
- if (cut_plane.intersects_segment(a, b, &inters)) {
+ if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) {
if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
path.push_back(inters);
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-11-04 01:06:02 +0000