diff options
Diffstat (limited to 'modules/navigation/nav_map.cpp')
| -rw-r--r-- | modules/navigation/nav_map.cpp | 36 |
1 files changed, 25 insertions, 11 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 1151df6390..344475fb37 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -30,7 +30,6 @@ #include "nav_map.h" -#include "core/os/threaded_array_processor.h" #include "nav_region.h" #include "rvo_agent.h" @@ -142,10 +141,10 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p bool is_reachable = true; 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++) { + 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]; // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. @@ -226,6 +225,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p navigation_polys.push_back(np); to_visit.clear(); to_visit.push_back(0); + least_cost_id = 0; reachable_end = nullptr; @@ -245,6 +245,8 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p } } + ERR_BREAK(least_cost_id == -1); + // Stores the further reachable end polygon, in case our goal is not reachable. if (is_reachable) { float d = navigation_polys[least_cost_id].entry.distance_to(p_destination); @@ -254,8 +256,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p } } - ERR_BREAK(least_cost_id == -1); - // Check if we reached the end if (navigation_polys[least_cost_id].poly == end_poly) { found_route = true; @@ -351,11 +351,15 @@ 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) { - path.push_back(navigation_polys[np_id].entry); + while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { + 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; + path.push_back(point); np_id = navigation_polys[np_id].back_navigation_poly_id; } + path.push_back(begin_point); path.reverse(); } @@ -537,7 +541,7 @@ void NavMap::sync() { } // Group all edges per key. - Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections; + HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey> connections; for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) { gd::Polygon &poly(polygons[poly_id]); @@ -545,7 +549,7 @@ 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, Vector<gd::Edge::Connection>>::Element *connection = connections.find(ek); + HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey>::Iterator connection = connections.find(ek); if (!connection) { connections[ek] = Vector<gd::Edge::Connection>(); } @@ -669,7 +673,10 @@ void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) { void NavMap::step(real_t p_deltatime) { deltatime = p_deltatime; if (controlled_agents.size() > 0) { - thread_process_array( + if (step_work_pool.get_thread_count() == 0) { + step_work_pool.init(); + } + step_work_pool.do_work( controlled_agents.size(), this, &NavMap::compute_single_step, @@ -714,3 +721,10 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys } } } + +NavMap::NavMap() { +} + +NavMap::~NavMap() { + step_work_pool.finish(); +} |