asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge pull request #63616 from smix8/navigation_std_to_localvector_3.5

Browse source
This commit is contained in:
Rémi Verschelde 2022-07-29 13:43:06 +02:00 committed by GitHub
commit 38601ba824
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 63 additions and 57 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
modules/navigation/godot_navigation_server.cpp
View file

@ -263,8 +263,10 @@ Array GodotNavigationServer::map_get_regions(RID p_map) const {
Array regions_rids; Array regions_rids;
const NavMap *map = map_owner.getornull(p_map); const NavMap *map = map_owner.getornull(p_map);
ERR_FAIL_COND_V(map == nullptr, regions_rids); ERR_FAIL_COND_V(map == nullptr, regions_rids);
for (NavRegion *region : map->get_regions()) { const LocalVector<NavRegion *> regions = map->get_regions();
regions_rids.push_back(region->get_self()); regions_rids.resize(regions.size());
for (uint32_t i = 0; i < regions.size(); i++) {
regions_rids[i] = regions[i]->get_self();
} }
return regions_rids; return regions_rids;
} }
@ -273,8 +275,10 @@ Array GodotNavigationServer::map_get_agents(RID p_map) const {
Array agents_rids; Array agents_rids;
const NavMap *map = map_owner.getornull(p_map); const NavMap *map = map_owner.getornull(p_map);
ERR_FAIL_COND_V(map == nullptr, agents_rids); ERR_FAIL_COND_V(map == nullptr, agents_rids);
for (RvoAgent *agent : map->get_agents()) { const LocalVector<RvoAgent *> agents = map->get_agents();
agents_rids.push_back(agent->get_self()); agents_rids.resize(agents.size());
for (uint32_t i = 0; i < agents.size(); i++) {
agents_rids[i] = agents[i]->get_self();
} }
return agents_rids; return agents_rids;
} }
@ -553,15 +557,15 @@ COMMAND_1(free, RID, p_object) {
NavMap *map = map_owner.getornull(p_object); NavMap *map = map_owner.getornull(p_object);
// Removes any assigned region // Removes any assigned region
std::vector<NavRegion *> regions = map->get_regions(); LocalVector<NavRegion *> regions = map->get_regions();
for (size_t i(0); i < regions.size(); i++) { for (uint32_t i = 0; i < regions.size(); i++) {
map->remove_region(regions[i]); map->remove_region(regions[i]);
regions[i]->set_map(nullptr); regions[i]->set_map(nullptr);
} }
// Remove any assigned agent // Remove any assigned agent
std::vector<RvoAgent *> agents = map->get_agents(); LocalVector<RvoAgent *> agents = map->get_agents();
for (size_t i(0); i < agents.size(); i++) { for (uint32_t i = 0; i < agents.size(); i++) {
map->remove_agent(agents[i]); map->remove_agent(agents[i]);
agents[i]->set_map(nullptr); agents[i]->set_map(nullptr);
} }

2
modules/navigation/godot_navigation_server.h
View file

@ -67,7 +67,7 @@ class GodotNavigationServer : public NavigationServer {
/// Mutex used to make any operation threadsafe. /// Mutex used to make any operation threadsafe.
Mutex operations_mutex; Mutex operations_mutex;
std::vector<SetCommand *> commands; LocalVector<SetCommand *> commands;
mutable RID_Owner<NavMap> map_owner; mutable RID_Owner<NavMap> map_owner;
mutable RID_Owner<NavRegion> region_owner; mutable RID_Owner<NavRegion> region_owner;

74
modules/navigation/nav_map.cpp
View file

@ -124,7 +124,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
} }
// List of all reachable navigation polys. // List of all reachable navigation polys.
std::vector<gd::NavigationPoly> navigation_polys; LocalVector<gd::NavigationPoly> navigation_polys;
navigation_polys.reserve(polygons.size() * 0.75); navigation_polys.reserve(polygons.size() * 0.75);
// Add the start polygon to the reachable navigation polygons. // Add the start polygon to the reachable navigation polygons.
@ -177,20 +177,18 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly->entry, pathway); const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly->entry, pathway);
const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance; const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance;
const std::vector<gd::NavigationPoly>::iterator it = std::find( int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));
navigation_polys.begin(),
navigation_polys.end(),
gd::NavigationPoly(connection.polygon));
if (it != navigation_polys.end()) { if (already_visited_polygon_index != -1) {
// Polygon already visited, check if we can reduce the travel cost. // Polygon already visited, check if we can reduce the travel cost.
if (new_distance < it->traveled_distance) { gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index];
it->back_navigation_poly_id = least_cost_id; if (new_distance < avp.traveled_distance) {
it->back_navigation_edge = connection.edge; avp.back_navigation_poly_id = least_cost_id;
it->back_navigation_edge_pathway_start = connection.pathway_start; avp.back_navigation_edge = connection.edge;
it->back_navigation_edge_pathway_end = connection.pathway_end; avp.back_navigation_edge_pathway_start = connection.pathway_start;
it->traveled_distance = new_distance; avp.back_navigation_edge_pathway_end = connection.pathway_end;
it->entry = new_entry; avp.traveled_distance = new_distance;
avp.entry = new_entry;
} }
} else { } else {
// Add the neighbour polygon to the reachable ones. // Add the neighbour polygon to the reachable ones.
@ -477,15 +475,15 @@ void NavMap::add_region(NavRegion *p_region) {
} }
void NavMap::remove_region(NavRegion *p_region) { void NavMap::remove_region(NavRegion *p_region) {
const std::vector<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region); int64_t region_index = regions.find(p_region);
if (it != regions.end()) { if (region_index != -1) {
regions.erase(it); regions.remove_unordered(region_index);
regenerate_links = true; regenerate_links = true;
} }
} }
bool NavMap::has_agent(RvoAgent *agent) const { bool NavMap::has_agent(RvoAgent *agent) const {
return std::find(agents.begin(), agents.end(), agent) != agents.end(); return (agents.find(agent) != -1);
} }
void NavMap::add_agent(RvoAgent *agent) { void NavMap::add_agent(RvoAgent *agent) {
@ -497,15 +495,15 @@ void NavMap::add_agent(RvoAgent *agent) {
void NavMap::remove_agent(RvoAgent *agent) { void NavMap::remove_agent(RvoAgent *agent) {
remove_agent_as_controlled(agent); remove_agent_as_controlled(agent);
const std::vector<RvoAgent *>::iterator it = std::find(agents.begin(), agents.end(), agent); int64_t agent_index = agents.find(agent);
if (it != agents.end()) { if (agent_index != -1) {
agents.erase(it); agents.remove_unordered(agent_index);
agents_dirty = true; agents_dirty = true;
} }
} }
void NavMap::set_agent_as_controlled(RvoAgent *agent) { void NavMap::set_agent_as_controlled(RvoAgent *agent) {
const bool exist = std::find(controlled_agents.begin(), controlled_agents.end(), agent) != controlled_agents.end(); const bool exist = (controlled_agents.find(agent) != -1);
if (!exist) { if (!exist) {
ERR_FAIL_COND(!has_agent(agent)); ERR_FAIL_COND(!has_agent(agent));
controlled_agents.push_back(agent); controlled_agents.push_back(agent);
@ -513,22 +511,23 @@ void NavMap::set_agent_as_controlled(RvoAgent *agent) {
} }
void NavMap::remove_agent_as_controlled(RvoAgent *agent) { void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
const std::vector<RvoAgent *>::iterator it = std::find(controlled_agents.begin(), controlled_agents.end(), agent); int64_t active_avoidance_agent_index = controlled_agents.find(agent);
if (it != controlled_agents.end()) { if (active_avoidance_agent_index != -1) {
controlled_agents.erase(it); controlled_agents.remove_unordered(active_avoidance_agent_index);
agents_dirty = true;
} }
} }
void NavMap::sync() { void NavMap::sync() {
// Check if we need to update the links. // Check if we need to update the links.
if (regenerate_polygons) { if (regenerate_polygons) {
for (size_t r(0); r < regions.size(); r++) { for (uint32_t r = 0; r < regions.size(); r++) {
regions[r]->scratch_polygons(); regions[r]->scratch_polygons();
} }
regenerate_links = true; regenerate_links = true;
} }
for (size_t r(0); r < regions.size(); r++) { for (uint32_t r = 0; r < regions.size(); r++) {
if (regions[r]->sync()) { if (regions[r]->sync()) {
regenerate_links = true; regenerate_links = true;
} }
@ -536,33 +535,33 @@ void NavMap::sync() {
if (regenerate_links) { if (regenerate_links) {
// Remove regions connections. // Remove regions connections.
for (size_t r(0); r < regions.size(); r++) { for (uint32_t r = 0; r < regions.size(); r++) {
regions[r]->get_connections().clear(); regions[r]->get_connections().clear();
} }
// Resize the polygon count. // Resize the polygon count.
int count = 0; int count = 0;
for (size_t r(0); r < regions.size(); r++) { for (uint32_t r = 0; r < regions.size(); r++) {
count += regions[r]->get_polygons().size(); count += regions[r]->get_polygons().size();
} }
polygons.resize(count); polygons.resize(count);
// Copy all region polygons in the map. // Copy all region polygons in the map.
count = 0; count = 0;
for (size_t r(0); r < regions.size(); r++) { for (uint32_t r = 0; r < regions.size(); r++) {
std::copy( const LocalVector<gd::Polygon> &polygons_source = regions[r]->get_polygons();
regions[r]->get_polygons().data(), for (uint32_t n = 0; n < polygons_source.size(); n++) {
regions[r]->get_polygons().data() + regions[r]->get_polygons().size(), polygons[count + n] = polygons_source[n];
polygons.begin() + count); }
count += regions[r]->get_polygons().size(); count += regions[r]->get_polygons().size();
} }
// Group all edges per key. // Group all edges per key.
Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections; Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections;
for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) { for (uint32_t poly_id = 0; poly_id < polygons.size(); poly_id++) {
gd::Polygon &poly(polygons[poly_id]); gd::Polygon &poly(polygons[poly_id]);
for (size_t p(0); p < poly.points.size(); p++) { for (uint32_t p = 0; p < poly.points.size(); p++) {
int next_point = (p + 1) % poly.points.size(); int next_point = (p + 1) % poly.points.size();
gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key); gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
@ -669,6 +668,7 @@ void NavMap::sync() {
// Update agents tree. // Update agents tree.
if (agents_dirty) { if (agents_dirty) {
// cannot use LocalVector here as RVO library expects std::vector to build KdTree
std::vector<RVO::Agent *> raw_agents; std::vector<RVO::Agent *> raw_agents;
raw_agents.reserve(agents.size()); raw_agents.reserve(agents.size());
for (size_t i(0); i < agents.size(); i++) { for (size_t i(0); i < agents.size(); i++) {
@ -697,7 +697,7 @@ void NavMap::step(real_t p_deltatime) {
controlled_agents.size(), controlled_agents.size(),
this, this,
&NavMap::compute_single_step, &NavMap::compute_single_step,
controlled_agents.data()); controlled_agents.ptr());
} }
} }
@ -707,7 +707,7 @@ 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 { void NavMap::clip_path(const LocalVector<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]; Vector3 from = path[path.size() - 1];
if (from.is_equal_approx(p_to_point)) { if (from.is_equal_approx(p_to_point)) {

14
modules/navigation/nav_map.h
View file

@ -59,10 +59,10 @@ class NavMap : public NavRid {
bool regenerate_polygons = true; bool regenerate_polygons = true;
bool regenerate_links = true; bool regenerate_links = true;
std::vector<NavRegion *> regions; LocalVector<NavRegion *> regions;
/// Map polygons /// Map polygons
std::vector<gd::Polygon> polygons; LocalVector<gd::Polygon> polygons;
/// Rvo world /// Rvo world
RVO::KdTree rvo; RVO::KdTree rvo;
@ -71,10 +71,10 @@ class NavMap : public NavRid {
bool agents_dirty = false; bool agents_dirty = false;
/// All the Agents (even the controlled one) /// All the Agents (even the controlled one)
std::vector<RvoAgent *> agents; LocalVector<RvoAgent *> agents;
/// Controlled agents /// Controlled agents
std::vector<RvoAgent *> controlled_agents; LocalVector<RvoAgent *> controlled_agents;
/// Physics delta time /// Physics delta time
real_t deltatime = 0.0; real_t deltatime = 0.0;
@ -120,14 +120,14 @@ public:
void add_region(NavRegion *p_region); void add_region(NavRegion *p_region);
void remove_region(NavRegion *p_region); void remove_region(NavRegion *p_region);
const std::vector<NavRegion *> &get_regions() const { const LocalVector<NavRegion *> &get_regions() const {
return regions; return regions;
} }
bool has_agent(RvoAgent *agent) const; bool has_agent(RvoAgent *agent) const;
void add_agent(RvoAgent *agent); void add_agent(RvoAgent *agent);
void remove_agent(RvoAgent *agent); void remove_agent(RvoAgent *agent);
const std::vector<RvoAgent *> &get_agents() const { const LocalVector<RvoAgent *> &get_agents() const {
return agents; return agents;
} }
@ -144,7 +144,7 @@ public:
private: private:
void compute_single_step(uint32_t index, RvoAgent **agent); void compute_single_step(uint32_t index, RvoAgent **agent);
void 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; void clip_path(const LocalVector<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;
}; };
#endif // NAV_MAP_H #endif // NAV_MAP_H

4
modules/navigation/nav_region.h
View file

@ -53,7 +53,7 @@ class NavRegion : public NavRid {
bool polygons_dirty = true; bool polygons_dirty = true;
/// Cache /// Cache
std::vector<gd::Polygon> polygons; LocalVector<gd::Polygon> polygons;
public: public:
void scratch_polygons() { void scratch_polygons() {
@ -91,7 +91,7 @@ public:
Vector3 get_connection_pathway_start(int p_connection_id) const; Vector3 get_connection_pathway_start(int p_connection_id) const;
Vector3 get_connection_pathway_end(int p_connection_id) const; Vector3 get_connection_pathway_end(int p_connection_id) const;
std::vector<gd::Polygon> const &get_polygons() const { LocalVector<gd::Polygon> const &get_polygons() const {
return polygons; return polygons;
} }

6
modules/navigation/nav_utils.h
View file

@ -90,13 +90,13 @@ struct Polygon {
NavRegion *owner = nullptr; NavRegion *owner = nullptr;
/// The points of this `Polygon` /// The points of this `Polygon`
std::vector<Point> points; LocalVector<Point> points;
/// Are the points clockwise ? /// Are the points clockwise ?
bool clockwise; bool clockwise;
/// The edges of this `Polygon` /// The edges of this `Polygon`
std::vector<Edge> edges; LocalVector<Edge> edges;
/// The center of this `Polygon` /// The center of this `Polygon`
Vector3 center; Vector3 center;
@ -118,6 +118,8 @@ struct NavigationPoly {
/// The distance to the destination. /// The distance to the destination.
float traveled_distance = 0.0; float traveled_distance = 0.0;
NavigationPoly() { poly = nullptr; }
NavigationPoly(const Polygon *p_poly) : NavigationPoly(const Polygon *p_poly) :
poly(p_poly) {} poly(p_poly) {}