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;
const NavMap *map = map_owner.getornull(p_map);
ERR_FAIL_COND_V(map == nullptr, regions_rids);
for (NavRegion *region : map->get_regions()) {
regions_rids.push_back(region->get_self());
const LocalVector<NavRegion *> regions = map->get_regions();
regions_rids.resize(regions.size());
for (uint32_t i = 0; i < regions.size(); i++) {
regions_rids[i] = regions[i]->get_self();
}
return regions_rids;
}
@ -273,8 +275,10 @@ Array GodotNavigationServer::map_get_agents(RID p_map) const {
Array agents_rids;
const NavMap *map = map_owner.getornull(p_map);
ERR_FAIL_COND_V(map == nullptr, agents_rids);
for (RvoAgent *agent : map->get_agents()) {
agents_rids.push_back(agent->get_self());
const LocalVector<RvoAgent *> agents = map->get_agents();
agents_rids.resize(agents.size());
for (uint32_t i = 0; i < agents.size(); i++) {
agents_rids[i] = agents[i]->get_self();
}
return agents_rids;
}
@ -553,15 +557,15 @@ COMMAND_1(free, RID, p_object) {
NavMap *map = map_owner.getornull(p_object);
// Removes any assigned region
std::vector<NavRegion *> regions = map->get_regions();
for (size_t i(0); i < regions.size(); i++) {
LocalVector<NavRegion *> regions = map->get_regions();
for (uint32_t i = 0; i < regions.size(); i++) {
map->remove_region(regions[i]);
regions[i]->set_map(nullptr);
}
// Remove any assigned agent
std::vector<RvoAgent *> agents = map->get_agents();
for (size_t i(0); i < agents.size(); i++) {
LocalVector<RvoAgent *> agents = map->get_agents();
for (uint32_t i = 0; i < agents.size(); i++) {
map->remove_agent(agents[i]);
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 operations_mutex;
std::vector<SetCommand *> commands;
LocalVector<SetCommand *> commands;
mutable RID_Owner<NavMap> map_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.
std::vector<gd::NavigationPoly> navigation_polys;
LocalVector<gd::NavigationPoly> navigation_polys;
navigation_polys.reserve(polygons.size() * 0.75);
// 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 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(
navigation_polys.begin(),
navigation_polys.end(),
gd::NavigationPoly(connection.polygon));
int64_t already_visited_polygon_index = navigation_polys.find(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.
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;
it->entry = new_entry;
gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index];
if (new_distance < avp.traveled_distance) {
avp.back_navigation_poly_id = least_cost_id;
avp.back_navigation_edge = connection.edge;
avp.back_navigation_edge_pathway_start = connection.pathway_start;
avp.back_navigation_edge_pathway_end = connection.pathway_end;
avp.traveled_distance = new_distance;
avp.entry = new_entry;
}
} else {
// 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) {
const std::vector<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region);
if (it != regions.end()) {
regions.erase(it);
int64_t region_index = regions.find(p_region);
if (region_index != -1) {
regions.remove_unordered(region_index);
regenerate_links = true;
}
}
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) {
@ -497,15 +495,15 @@ void NavMap::add_agent(RvoAgent *agent) {
void NavMap::remove_agent(RvoAgent *agent) {
remove_agent_as_controlled(agent);
const std::vector<RvoAgent *>::iterator it = std::find(agents.begin(), agents.end(), agent);
if (it != agents.end()) {
agents.erase(it);
int64_t agent_index = agents.find(agent);
if (agent_index != -1) {
agents.remove_unordered(agent_index);
agents_dirty = true;
}
}
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) {
ERR_FAIL_COND(!has_agent(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) {
const std::vector<RvoAgent *>::iterator it = std::find(controlled_agents.begin(), controlled_agents.end(), agent);
if (it != controlled_agents.end()) {
controlled_agents.erase(it);
int64_t active_avoidance_agent_index = controlled_agents.find(agent);
if (active_avoidance_agent_index != -1) {
controlled_agents.remove_unordered(active_avoidance_agent_index);
agents_dirty = true;
}
}
void NavMap::sync() {
// Check if we need to update the links.
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();
}
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()) {
regenerate_links = true;
}
@ -536,33 +535,33 @@ void NavMap::sync() {
if (regenerate_links) {
// 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();
}
// Resize the polygon count.
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();
}
polygons.resize(count);
// 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);
for (uint32_t r = 0; r < regions.size(); r++) {
const LocalVector<gd::Polygon> &polygons_source = regions[r]->get_polygons();
for (uint32_t n = 0; n < polygons_source.size(); n++) {
polygons[count + n] = polygons_source[n];
}
count += regions[r]->get_polygons().size();
}
// 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++) {
for (uint32_t poly_id = 0; poly_id < polygons.size(); 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();
gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
@ -669,6 +668,7 @@ void NavMap::sync() {
// Update agents tree.
if (agents_dirty) {
// cannot use LocalVector here as RVO library expects std::vector to build KdTree
std::vector<RVO::Agent *> raw_agents;
raw_agents.reserve(agents.size());
for (size_t i(0); i < agents.size(); i++) {
@ -697,7 +697,7 @@ void NavMap::step(real_t p_deltatime) {
controlled_agents.size(),
this,
&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];
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_links = true;
std::vector<NavRegion *> regions;
LocalVector<NavRegion *> regions;
/// Map polygons
std::vector<gd::Polygon> polygons;
LocalVector<gd::Polygon> polygons;
/// Rvo world
RVO::KdTree rvo;
@ -71,10 +71,10 @@ class NavMap : public NavRid {
bool agents_dirty = false;
/// All the Agents (even the controlled one)
std::vector<RvoAgent *> agents;
LocalVector<RvoAgent *> agents;
/// Controlled agents
std::vector<RvoAgent *> controlled_agents;
LocalVector<RvoAgent *> controlled_agents;
/// Physics delta time
real_t deltatime = 0.0;
@ -120,14 +120,14 @@ public:
void add_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;
}
bool has_agent(RvoAgent *agent) const;
void add_agent(RvoAgent *agent);
void remove_agent(RvoAgent *agent);
const std::vector<RvoAgent *> &get_agents() const {
const LocalVector<RvoAgent *> &get_agents() const {
return agents;
}
@ -144,7 +144,7 @@ public:
private:
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

4
modules/navigation/nav_region.h
View file

@ -53,7 +53,7 @@ class NavRegion : public NavRid {
bool polygons_dirty = true;
/// Cache
std::vector<gd::Polygon> polygons;
LocalVector<gd::Polygon> polygons;
public:
void scratch_polygons() {
@ -91,7 +91,7 @@ public:
Vector3 get_connection_pathway_start(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;
}

6
modules/navigation/nav_utils.h
View file

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