Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make navigation map spatial queries thread-safe #79577

Merged
merged 1 commit into from
Feb 12, 2024
Merged

Make navigation map spatial queries thread-safe #79577

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 28 additions & 5 deletions modules/navigation/nav_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,11 @@ 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, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector<Vector3>(), "NavigationServer map query failed because it was made before first map synchronization.");
RWLockRead read_lock(map_rwlock);
if (map_update_id == 0) {
ERR_FAIL_V_MSG(Vector<Vector3>(), "NavigationServer map query failed because it was made before first map synchronization.");
}

// Clear metadata outputs.
if (r_path_types) {
r_path_types->clear();
Expand Down Expand Up @@ -576,7 +580,11 @@ 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 {
ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
RWLockRead read_lock(map_rwlock);
if (map_update_id == 0) {
ERR_FAIL_V_MSG(Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
}

bool use_collision = p_use_collision;
Vector3 closest_point;
real_t closest_point_d = FLT_MAX;
Expand Down Expand Up @@ -624,24 +632,35 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
}

Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
RWLockRead read_lock(map_rwlock);
if (map_update_id == 0) {
ERR_FAIL_V_MSG(Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
}
gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
return cp.point;
}

Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
RWLockRead read_lock(map_rwlock);
if (map_update_id == 0) {
ERR_FAIL_V_MSG(Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
}
gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
return cp.normal;
}

RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
ERR_FAIL_COND_V_MSG(map_update_id == 0, RID(), "NavigationServer map query failed because it was made before first map synchronization.");
RWLockRead read_lock(map_rwlock);
if (map_update_id == 0) {
ERR_FAIL_V_MSG(RID(), "NavigationServer map query failed because it was made before first map synchronization.");
}
gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
return cp.owner;
}

gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
RWLockRead read_lock(map_rwlock);

gd::ClosestPointQueryResult result;
real_t closest_point_ds = FLT_MAX;

Expand Down Expand Up @@ -770,6 +789,8 @@ void NavMap::remove_agent_as_controlled(NavAgent *agent) {
}

Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
RWLockRead read_lock(map_rwlock);

const LocalVector<NavRegion *> map_regions = get_regions();

if (map_regions.is_empty()) {
Expand Down Expand Up @@ -834,6 +855,8 @@ Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly)
}

void NavMap::sync() {
RWLockWrite write_lock(map_rwlock);

// Performance Monitor
int _new_pm_region_count = regions.size();
int _new_pm_agent_count = agents.size();
Expand Down
2 changes: 2 additions & 0 deletions modules/navigation/nav_map.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ class NavAgent;
class NavObstacle;

class NavMap : public NavRid {
RWLock map_rwlock;

/// Map Up
Vector3 up = Vector3(0, 1, 0);

Expand Down
Loading